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Pose Graph Optimization in the Complex Domain: 
Lagrangian Duality, Conditions For Zero 
Duality Gap, and Optimal Solutions 

Giuseppe C. Calafiore*, Luca Carlonet and Frank Dellaert* 


Abstract 

Pose Graph Optimization (PGO) is the problem of estimating a 
set of poses from pairwise relative measurements. PGO is a nonconvex 
problem, and currently no known technique can guarantee the com¬ 
putation of a global optimal solution. In this paper, we show that 
Lagrangian duality allows computing a globally optimal solution, un¬ 
der certain conditions that are satisfied in many practical cases. Our 
first contribution is to frame the PGO problem in the complex do¬ 
main. This makes analysis easier and allows drawing connections with 
the recent literature on unit gain graphs. Exploiting this connection 
we prove nontrival results about the spectrum of the matrix underly¬ 
ing the problem. The second contribution is to formulate and analyze 
the properties of the Lagrangian dual problem in the complex domain. 

The dual problem is a semidefinite program (SDP). Our analysis shows 
that the duality gap is connected to the number of eigenvalues of the 
penalized pose graph matrix , which arises from the solution of the SDP. 

We prove that if this matrix has a single eigenvalue in zero , then (i) 
the duality gap is zero, (ii) the primal PGO problem has a unique 
solution, and (iii) the primal solution can be computed by scaling an 
eigenvector of the penalized pose graph matrix. The third contribution 
is algorithmic: we exploit the dual problem and propose an algorithm 
that computes a guaranteed optimal solution for PGO when the penal¬ 
ized pose graph matrix satisfies the Single Zero Eigenvalue Property 
(SZEP). We also propose a variant that deals with the case in which 
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the SZEP is not satisfied. This variant, while possibly suboptimal, 
provides a very good estimate for PGO in practice. The fourth con¬ 
tribution is a numerical analysis. Empirical evidence shows that in 
the vast majority of cases (100% of the tests under noise regimes of 
practical robotics applications) the penalized pose graph matrix does 
satisfy the SZEP, hence our approach allows computing the global op¬ 
timal solution. Finally, we report simple counterexamples in which the 
duality gap is nonzero, and discuss open problems. 


1 Introduction 

Pose graph optimization (PGO) consists in the estimation of the poses (posi¬ 
tions and orientations) of a mobile robot, from relative pose measurements. 
The problem can be formulated as the minimization of a nonconvex cost, 
and can be conveniently visualized as a graph, in which a (to-be-estimated) 
pose is attached to each vertex, and a given relative pose measurement is 
associated to each edge. 

PGO is a key problem in many application endeavours. In robotics, it 
lies at the core of state-of-the-art algorithms for localization and mapping in 
both single robot [50 GTJ EH ETJ [371 HU EH EH E0 [13] and multi robot [4(11 
m EH ESI EH systems. In computer vision and control, problems that are 
closely related to PGO need to be solved for structure from motion [351 
m ei Ea m eh eh, attitude synchronization [731 [391 [56], camera network 
calibration [73 hm, sensor network localization [591 EH], and distributed 
consensus on manifolds [Ml [75]. Moreover, similar formulations arise in 
molecule structure determination from microscopy imaging [70 E]. 

A motivating example in robotics is the one pictured in Fig.Qa). A mo¬ 
bile robot is deployed in an unknown environment at time t = 0. The robot 
traverses the environment and at each discrete time step acquires a sen¬ 
sor measurement (e.g., distances from obstacles within the sensing radius). 
From wheel rotation, the robot is capable of measuring the relative motion 
between two consecutive poses (say, at time i and j). Moreover, compar¬ 
ing the sensor measurement, acquired at different times, the robot can also 
extrapolate relative measurements between non consecutive poses (e.g., be¬ 
tween i and k in the figure). PGO uses these measurements to estimate robot 
poses. The graph underlying the problem is shown in Fig. [IJb), where we 
draw in different colors the edges due to relative motion measurements (the 
odometric edges , in black) and the edges connecting non-consecutive poses 
(the loop closures, in red). The importance of estimating the robot poses 
is two-fold. First, the knowledge of the current robot pose is often needed 
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Figure 1: (a) Pose graph optimization in robotics. A mobile robot is 

deployed in an unknown environment at time t = 0. At each time step 
the robot measures distances from obstacles within the sensing radius (red 
circle). The sensor footprint (i.e., the set of measurements) at time T is 
visualized in orange. By matching sensor footprints acquired at different 
time steps, the robot establishes relative measurements between poses along 
its trajectory. PGO consists in the estimation of robot poses from these 
relative measurements, (b) Directed graph underlying the problem. 


for performing high-level tasks within the environment. Second, from the 
knowledge of all past poses, the robot can register all sensor footprints in a 
common frame, and obtain a map of the environment, which is needed for 
model-based navigation and path planning. 

Related work in robotics. Since the seminal paper [53j, PGO at¬ 
tracted large attention from the robotics community. Most state-of-the-art 
techniques currently rely on iterative nonlinear optimization, which refines 
a given initial guess. The Gauss-Newton method is a popular choice m 
m HI] , as it converges quickly when the initialization is close to a minimum 
of the cost function. Trust region methods (e.g., the Levenberg-Marquart 
method, or Powell’s Dog-Leg method [52j) have also been applied success¬ 
fully to PGO Til ;62] : the gradient method has been shown to have a large 
convergence basin, while suffering from long convergence tails [571 GUI0H]. A 
large body of literature focuses on speeding up computation. This includes 
exploiting sparsity [H3E23, using reduction schemes to limit the number of 
poses [501 El, faster linear solvers [Ml [22 ] , or approximate solutions [241 [12] . 

PGO is a nonconvex problem and iterative optimization techniques can 
only guarantee local convergence. State-of-the-art iterative solvers fail to 
converge to a global minimum of the cost for relatively small noise lev¬ 
els pans!. This fact recently triggered efforts towards the design of more 
robust techniques, together with a theoretical analysis of PGO. Huang et. al 
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m discuss the number of local minima in small PGO problems. Knuth and 
Barooah [49| investigate the growth of the error in absence of loop closures. 
Carlone [10] provides conservative approximations of the basin of conver¬ 
gence for the Gauss-Newton method. Huang et. al [30] and Wang et. al 
m discuss the nonlinearities in PGO. In order to improve global con¬ 
vergence, a successful strategy consists in solving for the rotations first, 
and then using the resulting estimate to bootstrap iterative methods for 
PGO 11J_. T2l [T3l H5| . This is convenient because the rotation subproblen^ 
can be solved in closed-form in 2D PI. and many heuristic algorithms for 
rotation estimation also perform well in 3D [55l Eg G21 mg. Despite the 
empirical success of state-of-the-art techniques, no approach can guarantee 
global convergence. It is not even known if the global optimizer itself is 
unique in general instances (while it is known that the minimizer is unique 
with probability one in the rotation subproblem |13|). The lack of guaran¬ 
tees promoted a recent interest in verification techniques for PGO. Carlone 
and Dellaert [H] use duality to evaluate the quality of a candidate solution 
in planar PGO. The work m also provides empirical evidence that in many 
problem instances the duality gap, i.e., the mismatch between the optimal 
cost of the primal and the dual problem, is zero. 

Related work in other fields. Variations of the PGO problem appear 
in different research fields. In computer vision, a somehow more difficult 
variant of the problem is known as bundle adjustment [33 ESI El ES s SB! ESI 
|33] . Contrarily to PGO, in bundle adjustment the relative measurements 
between the (camera) poses are only known up to scale. While no closed- 
form solution is known for bundle adjustment, many authors focused on 
the solution of the rotation subproblem [35] 155] [2] [36] 168] [33] [38] . The 
corresponding algorithms have excellent performance in practice, but they 
come with little guarantees, as they are based on relaxation. Fredriksson 
and Olsson [33] use duality theory to design a verification technique for 
quaternion-based rotation estimation. 

Related work in multi robot systems and sensor networks also includes 
contributions on rotation estimation (also known as attitude synchroniza¬ 
tion [ng m Ennm gd- Borra et. al [6] propose a distributed algorithm 
for planar rotation estimation. Tron and Vidal [771171] provide convergence 
results for distributed attitude consensus using gradient descent; distributed 
consensus on manifold [M] is related to estimation from relative measure- 

1 We use the term “rotation subproblem” to denote the problem of associating a rota¬ 
tion to each node in the graph, using relative rotation measurements. This corresponds 
to disregarding the translation measurements in PGO. 
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ments, as discussed in m- a problem that is formally equivalent to PGO 
is discussed in m eh] with application to sensor network localization. Pi- 
ovan et. al [59] provide observability conditions and discuss iterative algo¬ 
rithms that reduce the effect of noise. Peters et. al |58j study pose estima¬ 
tion in graphs with a single loop (related closed-form solutions also appear in 
other literatures [68j [25]), and provide an estimation algorithm over general 
graphs, based on the limit of a set of continuous-time differential equations, 
proving its effectiveness through numerical simulations. We only mention 
that a large literature in sensor network localization also deals with other 
types of relative measurements |53], including relative positions (with known 
rotations) [T[ [53]. relative distances [MIEnuilISlIMUHUHniEI] and relative 
bearing measurements HASHES]. 

A less trivial connection can be established with related work in molec¬ 
ular structure determination from cryo-electron microscopy mm, which 
offers very lucid and mature treatment of rotation estimation. Singer and 
Shkolnisky PHE] provide two approaches for rotation estimation, based 
on relaxation and semidefinite programming (SDP). Another merit of [70]] 
is to draw connections between planar rotation estimation and the “max- 
2-lin mod l” problem in combinatorial optimization, and “max-k-cut” 
problem in graph theory. Bandeira et. al [3] provide a Cheeger-like inequal¬ 
ity that establishes performance bounds for the SDP relaxation. Saunder- 
son et. al JBELEIj propose a tighter SDP relaxation, based on a spectrahedral 
representation of the convex hull of the rotation group. 

Contribution. This paper shows that the use of Lagrangian duality 
allows computing a guaranteed globally optimal solution for PGO in many 
practical cases, and proves that in those cases the solution is unique. 

Section [2] recalls preliminary concepts, and discusses the properties of a 
particular set of 2 x 2 matrices, which are scalar multiples of a planar rotation 
matrix. These matrices are omnipresent in planar PGO and acknowledging 
this fact allows reformulating the problem over complex variables. 

Section [3] frames PGO as a problem in complex variables. This makes 
analysis easier and allows drawing connections with the recent literature on 
unit gain graphs lag. Exploiting this connection we prove nontrival results 
about the spectrum of the matrix underlying the problem (the pose graph 
matrix ), such as the number of zero eigenvalues in particular graphs. 

Section [4] formulates the Lagrangian dual problem in the complex do¬ 
main. Moreover it presents an SDP relaxation of PGO, interpreting the 
relaxation as the dual of the dual problem. Our SDP relaxation is related 
to the one of mm, but we deal with 2D poses, rather than rotations; 
moreover, we only use the SDP relaxation to complement our discussion on 
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duality and to support some of the proofs. Section [4~3| contains keys results 
that relate the solution of the dual problem to the primal PGO problem. 
We show that the duality gap is connected to the zero eigenvalues of the 
penalized pose graph matrix , which arises from the solution of the dual prob¬ 
lem. We prove that if this matrix has a single eigenvalue in zero, then (i) 
the duality gap is zero, (ii) the primal PGO problem has a unique solution 
(up to an arbitrary roto-translation), and (iii) the primal solution can be 
computed by scaling the eigenvector of the penalized pose graph matrix cor¬ 
responding to the zero eigenvalue. To the best of our knowledge, this is the 
first work to discuss the uniqueness of the PGO solution for general graphs 
and to provide a provably optimal solution. 

Section [5] exploits our analysis of the dual problem to devise compu¬ 
tational approaches for PGO. We propose an algorithm that computes a 
guaranteed optimal solution for PGO when the penalized pose graph ma¬ 
trix satisfies the Single Zero Eigenvalue Property (SZEP). We also propose 
a variant that deals with the case in which the SZEP is not satisfied. This 
variant, while possibly suboptimal, is shown to perform well in practice, 
outperforming related approaches. 

Section [6] elucidates on our theoretical results with numerical tests. In 
practical regimes of operation (rotation noise < 0.3 rad and translation 
noise < 0.5 m), our Monte Carlo runs always produced a penalized pose 
graph matrix satisfying the SZEP. Hence, in all tests with reasonable noise 
our approach enables the computation of the optimal solution. For larger 
noise levels (e.g., 1 rad standard deviation for rotation measurements), we 
observed cases in which the penalized pose graph matrix has multiple eigen¬ 
values in zero. To stimulate further investigation towards structural results 
on duality (e.g., maximum level of noise for which the duality gap is provably 
zero) we report simple examples in which the duality gap is nonzero. 

2 Notation and preliminary concepts 

Section |2.1| introduces our notation. Section |2.2| recalls standard concepts 
from graph theory, and can be safely skipped by the expert reader. Sec¬ 
tion |2.3j instead, discusses the properties of the set of 2 x 2 matrices that 
are multiples of a planar rotation matrix. We denote this set with the sym¬ 
bol aSO( 2). The set aSO{ 2) is of interest in this paper since the action of 
any matrix Z E aSO{ 2) can be conveniently represented as a multiplication 
between complex numbers, as discussed in Section [3.3| Table [l] summarizes 
the main symbols used in this paper. 
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2.1 Notation 


The cardinality of a set V is written as |V|. The sets of real and complex 
numbers are denoted with M and C, respectively. I n denotes the n x n 
identity matrix, l n denotes the (column) vector of all ones of dimension 
n, Onxm denotes the n x m matrix of all zeros (we also use the shorthand 
0 n = Onx 1 ) • For a matrix M, Mij denotes the element of M in row i and 
column j. For matrices with a block structure we use [M]ij to denote the 
d x d block of M at the block row i and block column j. In this paper we 
only deal with matrices that have 2x2 blocks, i.e., d = 2, hence the notation 
[M]ij is unambiguous. 


2.2 Graph terminology 

A directed graph Q is a pair (V, £), where the vertices or nodes V are a finite 
set of elements, and £ C V x V is the set of edges. Each edge is an ordered 
pair e = (i. j). We say that e is incident on nodes i and j, leaves node i, 
called tail , and is directed towards node j, called head. The number of nodes 
is denoted with n = |V|, while the number of edges is m = \£\. 

The incidence matrix A of a directed graph is a m x n matrix with ele¬ 
ments in { —1,0,+1} that exhaustively describes the graph topology. Each 
row of A corresponds to an edge and has exactly two non-zero elements. For 
the row corresponding to edge e = (i, j), there is a —1 on the i-th column 
and a +1 on the j’-th column. 

The set of outgoing neighbors of node i is A f° ut = {j : (i,j) G £}■ The set 
of incoming neighbors of node i is A f- n = {j : (j, i) G £}. The set of neighbors 
of node i is the union of outgoing and incoming neighbors A/) = A/) out U A/) 111 . 


2.3 The set aSO{ 2) 

The set aSO( 2) is defined as 

aSO{ 2) = {aR : a G R, R G 50(2)}, 


where 50(2) is the set of 2D rotation matrices. Recall that 50(2) can be 
parametrized by an angle 6 G (—7r, +7r], and any matrix R G 50(2) is in the 
form: 


R = R(0) 


cos (9) — sin(0) 

sin(0) cos(0) 


(1) 


Clearly, 50(2) C a50(2). The set a50(2) is closed under standard ma¬ 
trix multiplication , i.e., for any Z\,Zi G a50(2), also the product Z\Z 2 G 
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aSO{ 2). In full analogy with 50(2), it is also trivial to show that the 
multiplication is commutative, i.e., for any Z\,Z 2 G aSO( 2) it holds that 
Z 1 Z 2 = Z 2 Z 1 . Moreover, for Z = aR with R G 50(2) it holds that 
Z T Z = \a\ 2 l 2 - The set aSO( 2 ) is also closed under matrix addition, since 
for R\, i ?2 £ 50(2), we have that 


Ot\R\ + CX2R2 — Q!l 

Cl 

-Si 

+ a 2 

C2 -S2 


S 1 

C 1 


S 2 C 2 


a\C\ + (X 2 C 2 

— (ausi + CC2S2) 


a —b 

ausi + (X 2 S 2 

aici + (X 2 C 2 


b a 


( 2 ) 


CU3R3 , 


where we used the shorthands c* and Sj for cos (9i) and sin( 0 j), and we defined 
a = aici + 0 . 2 C 2 and b = a 1 s 1 + « 2 S 2 - In Q, the scalar 03 = ±\/a 2 + b 2 (if 


nonzero) normalizes 


a/a 3 -b/a3 
b/013 a/ c*3 


is a rotation 


such that R 3 = 

matrix; if 03 = 0 , then a\R±+a 2 R 2 = 02 x 2 ) which also falls in our definition 
of 0:50(2). From this reasoning, it is clear that an alternative definition of 
o50(2) is 

a —b 
b a 


aSO( 2 ) = 


: a, b G 


(3) 


o50(2) is tightly coupled with the set of complex numbers C. Indeed, a 
matrix in the form ([ 3 ]) is also known as a matrix representation of a complex 
number [29]. We explore the implications of this fact for PGO in Section 3.3 


3 Pose graph optimization in the complex domain 
3.1 Standard PGO 

PGO estimates n poses from m relative pose measurements. We focus on the 
planar case, in which the z-th pose x t is described by the pair Xi = ( Pi,R %), 
where pi gK 2 is a position in the plane, and Rj G 50(2) is a planar rotation. 
The pose measurement between two nodes, say i and j, is described by the 
pair ( Aij , Rij), where A ij gK 2 and Rij g 50(2) are the relative position and 
rotation measurements, respectively. 

The problem can be visualized as a directed graph Q(y,£), where an 
unknown pose is attached to each node in the set V, and each edge ( i,j ) G £ 
corresponds to a relative pose measurement between nodes i and j (Fig. [ 2 ]). 
In a noiseless case, the measurements satisfy: 

= R J (Pj - Pi ), Rij = RjRj ) ( 4 ) 
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Table 1: Symbols used in this paper 


Graph 


Q = (v,e) 

Directed graph 

m 

Number of edges 

n 

Number of nodes 

V 

Vertex set; |V| = n 

£ 

Edge set; \£\ = m 

e = (■ i,j ) G £ 

Edge between nodes i and j 

A G R nxm 

Incidence matrix of Q 

A G 

Anchored incidence matrix of Q 

£ = A t A 

Laplacian matrix of Q 

L = A r A 

Anchored Laplacian matrix of Q 

Real PGO formulation 


A = A® h 

Augmented incidence matrix 

A = A (g) I 2 

Augmented anchored incidence matrix 

C = C ® 1-2 

Augmented Laplacian matrix 

W G M 4nx4n 

Real pose graph matrix 

^ g ]j( 4 n - 2 )x( 4 n- 2 ) 

Real anchored pose graph matrix 

p G M 2n 

Node positions 

p G M 2(n-1 ) 

Anchored node positions 

r G M 2n 

Node rotations 

Complex PGO formulation 

W g C(2n—l)x(2n—1) 

Complex anchored pose graph matrix 

p G C"- 1 

Anchored complex node positions 

f G C n 

Complex node rotations 

Miscellanea 


SO{ 2) 

2D rotation matrices 

aSO(2) 

Scalar multiple of a 2D rotation matrix 

|V| 

Cardinality of the set V 

In 

n x n identity matrix 

On (In) 

Column vector of zeros (ones) of dimension n 

Tr (X) 

Trace of the matrix X 
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Figure 2: Schematic representation of Pose Graph Optimization: the objec¬ 
tive is to associate a pose Xi to each node of a directed graph, given relative 
pose measurements (Ay, R. t] ) for each edge (i,j) in the graph. 


and we can compute the unknown rotations {R\,. ■., R n } and positions 
{pi,... ,p n } by solving a set of linear equations (relations Q become linear 
after rearranging the rotation Ri to the left-hand side). In absence of noise, 
the problem admits a unique solution as long as one fixes the pose of a node 
(say pi = O 2 and Ri = I 2 ) and the underling graph is connected. 

In this work we focus on connected graphs, as these are the ones of 
practical interest in PGO (a graph with k connected components can be 
split in k subproblems, which can be solved and analyzed independently). 

Assumption 1 (Connected Pose Graph) The graph Q underlying the 
pose graph optimization problem is connected. 

In presence of noise, the relations Q cannot be met exactly and pose 
graph optimization looks for a set of positions {p±,... ,p n } and rotations 
{/? 1 ,..., R n } that minimize the mismatch with respect to the measurements. 
This mismatch can be quantified by different cost functions. We adopt the 
formulation proposed in |14| : 

min ^ || Ay - R.J(pj - pi)\\\ + -||Rij ~ RjR jWf, (5) 

where || • H 2 is the standard Euclidean distance and || • || f is the Frobenius 
norm. The Frobenius norm \\R 0 — R.b\\ is a standard measure of distance 
between two rotations R a and Rb, and it is commonly referred to as the 
chordal distance, see, e.g., [38j. In ([ 5 ]), we used the short-hand notation 
{pi} (resp. {-R;}) to denote the set of unknown positions {p\ ... ,p n } (resp. 
rotations). 






Rearranging the terms, problem ([ 5 ]) can be rewritten as: 


min 

{. Pi },{Ri}eSO(2 )" 


^ ^ II (j?j Pi) Ri^ij 11 2 


T 211 Rj R /Rij 


112 
||F> 


( 6 ) 


where we exploited the fact that the 2-norm is invariant to rotation, i.e., 
for any vector v and any rotation matrix R it holds | Rv \ | 2 = ||u|| 2 - Eq. <© 
highlights that the objective is a quadratic function of the unknowns. 

The complexity of the problem stems from the fact that the constraint 
Ri E 50(2) is nonconvex, see, e.g., [65]. To make this more explicit, we 
follow the line of m, and use a more convenient representation for nodes’ 
rotations. Every planar rotation Ri can be written as in ([!]), and is fully 
defined by the vector 


cos(<9j) 

sin(0j) 


(7) 


Using this parametrization and with simple matrix manipulation, Eq. © 
becomes (c/. with Eq. (11) in [14]): 


min 

{Pi}i{ r i} 

s.t.: 


E (i,j)e£ II iPj-Pi) - D ii r i\\l + \\rj - RijnWl 
= 1, i = 1 ,..., n 


r-II 2 - 

' * II2 


where we defined: 


Dij — 


A x - - A'.'. 

IJ IJ 

A y - A' 

13 l J 


(with Aij = [A?- Af/) 


( 8 ) 


(9) 


and where the constraints | r t \ \ \ = 1 specify that we look for vectors r t that 
represent admissible rotations (i.e., such that cos(0j) 2 + sin(0j) 2 = 1). 

Problem ([8]) is a quadratic problem with quadratic equality constraints. 
The latter are nonconvex, hence computing a local minimum of ([8]) is hard 
in general. There are two problem instances, however, for which it is easy 
to compute a global minimizer, which attains zero optimal cost. These two 
cases are recalled in Propositions [T|[2| 


Proposition 1 (Zero cost in trees) An optimal solution for a PGO prob¬ 
lem in the form ([8]) whose underlying graph is a tree attains zero cost. 


The proof is given in Appendix |8.1| Roughly speaking, in a tree, we 
can build an optimal solution by concatenating the relative pose measure¬ 
ments, and this solution annihilates the cost function. This comes with no 
surprises, as the chords (i.e., the extra edges, added to a spanning tree) are 
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indeed the elements that create redundancy and improve the pose estimate. 
However, also for graphs with chords, it is possible to attain the zero cost 
in problem 


Definition 1 (Balanced pose graph) A pose graph is balanced if the 
pose measurements compose to the identity along each cycle in the graph 


■_ i 


In a balanced pose graph, there exists a configuration that explains ex¬ 
actly the measurements, as formalized in the following proposition. 


Proposition 2 (Zero cost in balanced pose graphs) An optimal solu¬ 
tion for a balanced pose graph optimization problem attains zero cost. 


The proof is given in Appendix |8.2| The concept of balanced graph 
describes a noiseless setup, while in real problem instances the measurements 
do not compose to the identity along cycles, because of the presence of noise. 


We note the following fact, which will be useful in Section 3.2 


Proposition 3 (Coefficient matrices in PGO) Matrices Djj. 1-2, —I 2 - R-ij 
appearing in ©> belong to aSO(2 ). 

This fact is trivial, since Rij,l 2 £ SO( 2) C aSO( 2) (the latter also implies 
—I 2 £ aSO( 2)). Moreover, the structure of in ([ 9 ]) clearly falls in the 
definition of matrices in aSO( 2) given in <§• 


3.2 Matrix formulation and anchoring 

In this section we rewrite the cost function ([8]) in a more convenient matrix 
form. The original cost is: 

f(p,r)= life -Pi) ~ D ij r i\\l + \\ r j - R ij r i\\l ( 10 ) 


where we denote with p £ M 2n and r £ M 2n the vectors stacking all nodes 
positions and rotations, respectively. Now, let A £ M mxn denote the inci¬ 
dence matrix of the graph underlying the problem: if (i,j) is the fc-th edge, 

2 We use the somehow standard term “composition” to denote the group operation 
for SE( 2). For two poses T\ = (p\,R\) and T2 = (P 2 ,hb), the composition is T\ ■ T2 = 
(pi + R1P2, R1R2) PS]. Similarly, the identity element is (O2, T2)- 

3 When composing measurements along the loop, edge direction is important: for two 
consecutive edges ( i , k ) and (k, j) along the loop, the composition is Tij = Tik ■ Tkj, while 
if the second edge is in the form ( j , k), the composition becomes Tij = TT- • TZ k . 
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then Aki = — 1, Akj = +1. Let A = A <8> h £ K 2mx2n ; and denote with 
Ak 6 M 2x2n the fe-th block row of A. From the structure of A, it follows 
that AkP = Pj — Pi- Also, we define D E K 2mx2n as a block matrix where 
the k -th block row Dk E M 2x2n corresponding to the /c-th edge (£ j) is all 
zeros, except for a 2 x 2 block —Djj in the i-tli block column. Using the 


matrices A and D, the first sum in (10) can be written as: 


E 

(*b)e£ 


- pi) - D 


U 


rs 9 = 


Y2\\AkP + D k r\\l = \\Ap + Dr\\l (11) 


k= 1 


Similarly, we define U E M 2mx2n as a block matrix where the k -th block row 
Uk S M 2x2n corresponding to the fc-th edge (i,j) is all zeros, except for 2x2 
blocks in the i-th and j-th block columns, which are equal to —Rij and I 2 , 


respectively. Using U, the second sum in (10) becomes: 


E 


\ r j - Rijn ||| = W u kA\l = \\ Ur \ 

k=1 


Combining (11) and (12), the cost in (10) becomes: 
f(p, r ) = 


r ad' 


p 1 

[ ° u 


r 


P 

T 

' A T A 

a t d 

r 


d t A 

+ 

'Q 

'Cl 

U T U 

P 

T 

£ 

A t d ' 

p 

r 


D T A 

Q 

r 


( 12 ) 



P 


r 


(13) 


where we defined Q = D T D + C/ T f7 and C = A T A, to simplify notation. 
Note that, since A = A <8> I 2 , it is easy to show that £ = £ ® I 2 , where 
£ = A t A is the Laplacian matrix of the graph underlying the problem. A 
pose graph optimization instance is thus completely defined by the matrix 


W = 


£ 

D T A 


A T D 
Q 


^ jg4nx4n 


(14) 


From (13), W can be easily seen to be symmetric and positive semidefinite. 
Other useful properties of W are stated in the next proposition. 


Proposition 4 (Properties of W) 

inite, and 


Matrix W in (14) is positive semidef- 


1 . has at least two eigenvalues in zero; 
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2 . is composed by 2x2 blocks [W]ij, and each block is a multiple of a 
rotation matrix, i.e., [W]ij G aSO(2), Vi, j = 1,..., 2 n. Moreover, the 
diagonal blocks of W are nonnegative multiples of the identity matrix, 
i.e., \W]u = aah, an > 0. 

A formal proof of Proposition [4] is given in Appendix |8.3| An intuitive 
explanation of the second claim follows from the fact that (i) W contains 
sums and products of the matrices in the original formulation (|8| (which are 
in aSO( 2) according to Lemma [3), and (ii) the set aSO( 2) is closed under 
matrix sum and product (Section 2.3). 

The presence of two eigenvalues in zero has a very natural geometric 
interpretation: the cost function encodes inter-nodal measurements, hence it 
is invariant to global translations of node positions, i.e., f(p, r ) = f(p+p a , r), 
where p a = (l n <8> ^ 2)0 = [a T ••• a T ] T (n copies of a), with a G M 2 . 
Algebraically, this translates to the fact that the matrix (l n <S> I 2 ) G M 2nx2 
is in the null space of the augmented incidence matrix A, which also implies 
a two dimensional null space for W. 


Position anchoring In this paper we show that the duality properties in 
pose graph optimization are tightly coupled with the spectrum of the matrix 
W. We are particularly interested in the eigenvalues at zero, and from this 
perspective it is not convenient to carry on the two null eigenvalues of W 
(claim 1 of Proposition |4j), which are always present, and are due to an 
intrinsic observability issue. 

We remove the translation ambiguity by fixing the position of an arbi¬ 
trary node. Without loss of generality, we fix the position p\ of the first 
node to the origin, i.e., p\ =02- This process is commonly called anchoring. 
Setting pi = 0 is equivalent to removing the corresponding columns and 
rows from W, leading to the following “anchored” PGO problem: 



' 0 2 ' 

T |- 

f(r,p) = 

P 

W 


r 




T 

r 

P 

w 

p 

r 


r 


(15) 


where p is the vector p without its first two-elements vector p\, and W is 
obtained from W by removing the rows and the columns corresponding to 
p\. The structure of W is as follows: 


a t a a t d 


L 

s 

d t A q 


S T 

Q 


( 16 ) 
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where A = A <S) I2, and A is the anchored (or reduced) incidence matrix, 
obtained by removing the first column from A. see, e.g., |12j . On the right- 
hand-side of (16) we defined S = A T D and Z = A T A. 


We call W the real (anchored) pose graph matrix. W is still symmetric 
and positive semidefinite (it is a principal submatrix of a positive semidefi- 
nite matrix). Moreover, since W is obtained by removing a 2 x 4n block row 
and a 4n x 2 block column from W, it is still composed by 2 x 2 matrices in 
aSO( 2), as specified in the following remark. 


Remark 1 (Properties of W) The positive semidefinite matrix W in (16) 
is composed by 2 x 2 blocks [W]y, that are such that [W]ij G aSO(2), 
\/i,j = l,...,2n — 1. Moreover, the diagonal blocks of W are nonnegative 
multiples of the identity matrix, i.e., \W]a = a n l 2 , ct > 0. 


After anchoring, our PGO problem becomes: 

n T 


f* = min 

PfT 

s.t.: 


P 

r 


W 

= 1 , 


(17) 


r-||2 - 

' * II2 


i = 1 ,..., n 


3.3 To complex domain 

In this section we reformulate problem ( |17[ ), in which the decision variables 
are real vectors, into a problem in complex variables. The main motivation 
for this choice is that the real representation © is somehow redundant: as 
we will show in Proposition [7J each eigenvalue of W is repeated twice (multi¬ 
plicity 2 ), while the complex representation does not have this redundancy, 
making analysis easier. In the rest of this paper, any quantity marked with 
a tilde ( 7 ) lives in the complex domain C. 

Any real vector v G M 2 can be represented by a complex number v = r/e w , 
where j 2 = —1 is the imaginary unit , 77 = ||n ||2 and ip is the angle that v 
forms with the horizontal axis. We use the operator (-) v to map a 2-vector 
to the corresponding complex number, v = v v . When convenient, we adopt 
the notation v ~ v, meaning that v and v are the vector and the complex 
representation of the same number. 

The action of a real 2x2 matrix Z on a vector v G M 2 cannot be 
represented, in general, as a scalar multiplication between complex numbers. 
However, if Z G aSO{ 2), this is possible. To show this, assume that Z = 
aR(9), where R{0) is a counter-clockwise rotation of angle 9. Then, 

Z v = aR(9)v ~ z v, where z = ae? e . (18) 
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With slight abuse of notation we extend the operator (-) v to aSO( 2), such 
that, given Z = aR{9) £ aSO( 2), then Z v = ae 10 £ C. By inspection, one 
can also verify the following relations between the sum and product of two 
matrices Z\,Z 2 £ aSO( 2) and their complex representations Zf, Zf £ C: 

(Z x z 2 y = Zf Zf (Z 1 + Z 2 ) v = Z 1 v + Z 2 v . (19) 


We next discuss how to apply the machinery introduced so far to reformulate 
problem © in the complex domain. The variables in problem © are 
the vectors p £ and r £ R 2n that are composed by 2-vectors, i.e., 

p = [p~l ,..., Pn-i] 7 and r = [r^, • • •, ] T , where pi, ri £ R 2 . Therefore, we 
define the complex positions and the complex rotations: 


p = [pi,... ,p n -i] T G C n \ where: pi = pV 

r = [ri,..., r n \ T £ C n , where: fi = r V 


( 20 ) 


Using the complex parametrization (20), the constraints in © become: 


|fj|“ = 1, * = 1, ... ,71- 


( 21 ) 


Similarly, we would like to rewrite the objective as a function of p and f. 
This re-parametrization is formalized in the following proposition, whose 
proof is given in Appendix |8.4| 


Proposition 5 (Cost in the complex domain) For any pair ( p,r ), the 

cost function in (17) is such that: 


f{Pi r ) 


r 1 

T 




T 

r 1 

p 

w 

p 

— 

p 

w 

p 

r 


r 


r 


r 


( 22 ) 


where the vectors p and r are built from p and r as in (20) 
W £ (CUrc-TxUn- 1 ) ^ s suc fo Wij = [W]ij 


and the 
with i, j = 1,..., 2 n 


matrix 

- 1. 


Remark 2 (Real diagonal entries for W ) According to Remark [fj the 
diagonal blocks ofW are multiples of the identity matrix, i.e., [W]a = aal 2 . 
Therefore, the diagonal elements ofW are Wu = [W]- = an £ R. 


Proposition [5] enables us to rewrite problem © as: 


f* = min 

P 

T 

w 

p 

p,T 

r 


f 


s.t.: \ri\ 2 = 1, 


( 23 ) 
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i = 1,..., n. 


















We call W the complex (anchored) pose graph matrix. Clearly, the matrix 


W preserves the same block structure of W in (16): 


W = 


L S 
S* Q 


(24) 


where S* is the conjugate transpose of S, and L = A T A where A is the 
anchored incidence matrix. In Section [4] we apply Lagrangian duality to the 
problem (23). Before that, we provide results to characterize the spectrum 


of the matrices W and W, drawing connections with the recent literature 
on unit gain graphs, [60]. 


3.4 Analysis of the real and complex pose graph matrices 

In this section we take a closer look at the structure and the properties of the 


real and the complex pose graph matrices W and W. In analogy with (13) 


and (16), we write W as 


W = 


A r A a t d 
(. A t D)* U*U + D*D 



' A 

D ' 

* 

' A 

D ' 


0 

U 


0 

U 


(25) 


where U G C mxn and D G C mxn are the “complex versions 
in (13), i.e., they are obtained as Uij = [U]ij v 


of U and D 
and Dij = [D]ij v , Vi,j. 


The factorization (25) is interesting, as it allows to identify two impor¬ 
tant matrices that compose W: the first is A, the anchored incidence matrix 
that we introduced earlier; the second is U which is a generalization of the 
incidence matrix, as specified by Definition [2] and Lemma [l] in the following. 
Fig. I reports the matrices A and U for a toy example with four poses. 


Definition 2 (Unit gain graphs) A unit gain graph (see, e.g., W) is a 
graph in which to each orientation of an edge ( i,j) is assigned a complex 
number zij (with \zij\ = l), which is the inverse of the complex number A 
assigned to the opposite orientation ( j,i ). Moreover, a complex incidence 
matrix of a unit gain graph is a matrix in which each row corresponds to 
an edge and the row corresponding to edge e = (i,j) has —zij on the i-th 
column, +1 on the j-th colum, and zero elsewhere. 


Roughly speaking, a unit gain graph describes a problem in which we 
can “flip” the orientation of an edge by inverting the corresponding complex 
weight. To understand what this property means in our context, recall the 
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Incidence matrix: 



Xl 

Anchored Incidence matrix: 


-1 

+1 

0 

0 ' 

( 1 , 2 ) 
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+1 

0 

( 2 , 3 ) 

0 

0 

-1 

+1 

( 3 , 4 ) 

+1 

0 

0 

-1 

( 4 , 1 ) 

0 

+1 

0 

-1 
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Complex Incidence matrix: 


' +1 

0 

0 ■ 
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+1 

0 

0 
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+1 

0 
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_0J041 

+1 

0 
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Figure 3: Example of incidence matrix, anchored incidence matrix, and 
complex incidence matrix, for the toy PGO problem on the top left. If 
Rij = R(Oij) is the relative rotation measurement associated to edge 
then the matrix U can be seen as the incidence matrix of a unit gain graph 
with gain eassociated to each edge (i,j). 


definition and consider the following chain of equalities: 

\\Ur\\l = Y \\rj - RijViWl = Y \\ r i ~ R Jj r i\\l ( 26 ) 

which, written in the complex domain become: 

\\Ur\\% = Y \rj-^ 3 ri\ 2 = Y \fi-e- iei 3 f j \ 2 = Y ~ (27) 

(i,j)e£ (*d)6£ (*j)e£ 


Eq. (27) essentially says that the term ||[7f||2 does not change if we flip the 
orientation of an edge and invert the relative rotation measurement. The 
proof of the following lemma is straightforward from (27). 


Lemma 1 (Properties of U) Matrix U is a complex incidence matrix of 
a unit gain graph with weights Rij w = e J ^ i associated to each edge ( i,j). 


Our interest towards unit gain graphs is motivated by the recent results 
in [[60j on the spectrum of the incidence matrix of those graphs. Using these 
results, we can characterize the presence of eigenvalues in zero fo r the matrix 
W, as specified in the following proposition (proof in Appendix |8.5[). 
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Proposition 6 (Zero eigenvalues in W) The complex anchored pose graph 
matrix W has a single eigenvalue in zero if and only if the pose graph is bal¬ 
anced or is a tree. 

Besides analyzing the spectrum of W, it is of interest to understand how 
the complex matrix W relates to the real matrix W. The following propo¬ 
sition states that there is a tight correspondence between the eigenvalues of 
the real pose graph matrix W and its complex counterpart W. 

Proposition 7 (Spectrum of complex graph matrices) The 2(2n—1) 
eigenvalues of W are the 2n — 1 eigenvalues of W, repeated twice. 

See Appendix |8.6| for a proof. 


4 Lagrangian duality in PGO 

In the previous section we wrote the PGO problem in complex variables as 


per eq. (23). In the following, we refer to this problem as the primal PGO 
problem, that, defining x = [p T r T ] T , can be written in compact form as 


/* = min 

X 

s.t.: 


x'Wx 


(Primal problem) 


Xi 


2 _ 


= 1, i = n,... ,2n — 1, 


(28) 


In this section we derive the Lagrangian dual of (28), which is given in 


Section 4.1 Then, in Section 4.2, we discuss an SDP relaxation of (28), that 


can be interpreted as the dual of the dual problem. Finally, in Section 4.3 
we analyze the properties of the dual problem, and discuss how it relates 
with the primal PGO problem. 

4.1 The dual problem 


The Lagrangian of the primal problem (28) is 

n 

L(x, A) = x T Wx + - \x n+i -i \ 2 ) 


i =1 


where A* 6 M, i = 1,..., n, are the Lagrange multipliers (or dual variables). 
ucl 

L(x, A) = x T 


Recalling the structure of W from (|24|), the Lagrangian becomes: 

n 

t IT(A)x + ^ Ai, 


L S 
. S* Q( A) . 


x 


i =1 


= X 


i=l 
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where for notational convenience we defined 


Q(A) = Q — diag(Ai,..., A n ), W(X) = 

The dual function d : M n — > M is the inhmum of the Lagrangian with respect 
to x: 


L S 
S* Q( A) 


d( A) = inf L(x, A) = inf x T W(X)x + Xi, 

-t •r • ^ 


(29) 


i =1 


For any choice of A the dual function provides a lower bound on the optimal 
value of the primal problem [Tj, Section 5.1.3]. Therefore, the Lagrangian 
dual problem looks for a maximum of the dual function over A: 


d* = max d(X) = max inf ,t t 1T(A)x + A,, 

A Ax ^ 

i=l 


(30) 


The infimum over x of L(x, A) drifts to —oo unless W{ A) ^ 0. Therefore 
we can safely restrict the maximization to vectors A that are such that 
VF(A) 0; these are called dual-feasible. Moreover, at any dual-feasible 
A, the x minimizing the Lagrangian are those that make x~^W{X)x = 0. 
Therefore, (30) reduces to the following dual problem 


d * = max 
A 


s.t.: W{ A) y 0. 


(Dual problem) 


The importance of the dual problem is twofold. First, it holds that 

d*<r 


(31) 


(32) 


This property is called weak duality , see, e.g., 0 Section 5.2.2], For partic¬ 


ular problems the inequality (32) becomes an equality, and in such cases we 


say that strong duality holds. Second, since d( A) is concave (minimum of 


affine functions), the dual problem (31) is always convex in A, regardless the 


convexity properties of the primal problem. The dual PGO problem (31) is 
a semidehnite program (SDP). 

For a given A, we denote by <T(A) the set of x that attain the optimal 


value in problem (29), if any: 


X(X) = {xa G C- : L(xa, A) = min L(x, A) = minx W( A)x} 
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Since we already observed that for any dual-feasible A the points x that 
minimize the Lagrangian are such that x t IT( \)x = 0, it follows that: 

A(A) = {x E C 2n_1 : W(X)x = 0} = Kernel(W(A)), for A dual-feasible. 

(33) 

The following result ensures that if a vector in A(A) is feasible for the primal 
problem, then it is also an optimal solution for the PGO problem. 


Theorem 1 Given A E M n , if an x\ E A(A) is primal feasible, then x\ is 
primal optimal; moreover, A is dual optimal, and the duality gap is zero. 


A proof of this theorem is given in Appendix 8.7 


4.2 SDP relaxation and the dual of the dual 


We have seen that a lower bound d * on the optimal value f* of the pri¬ 
mal (28) can be obtained by solving the Lagrangian dual problem (31). 
Here, we outline another, direct, relaxation method to obtain such bound. 
Observing that x T Wx = Tr (lhra T ), we rewrite (28) equivalently as 


/* = min Tr WX (34) 

X,x 

s.t.: Tr EiX = 1, i = n ,..., 2n — 1, 

X = xx T . 


where E{ is a matrix that is zero everywhere, except for the z-th diagonal 
element, which is one. The condition X = xx r is equivalent to (i) X ^ 0 
and (ii) X has rank one. Thus, (34) is rewritten by eliminating x as 


min 

X 

Tr WX 


(35) 

s.t.: 

Tr E,X = 1, i = n,.. 

., 2n — 1, 



x y o 




rank(A) = 1. 




Dropping the rank constraint, which is non-convex, we obtain the following 
SDP relaxation (see, e.g., [80] 4 of the primal problem: 

s* = min Tr WX 

x 

s.t.: Tr EiX = 1, i = n ,..., 2n — 1, (^6) 

X h 0 
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which we can also rewrite as 


min TV WX 

x 

s.t.: Xu = 1, 

x y o 


(SDP relaxation) 


i = n ,..., 2n — 1, 


(37) 


where Xu denotes the z-th diagonal entry in X. Obviously, s* < /*, since 
the feasible set of (37) contains that of (35). One may then ask what is 


the relation between the Lagrangian dual and the SDP relaxation of prob¬ 


lem (37): the answer is that the former is the dual of the latter hence, under 


constraint qualification, it holds that s* = d*, i.e., the SDP relaxation and 
the Lagrangian dual approach yield the same lower bound on /*. This is 
formalized in the following proposition. 


Proposition 8 The Lagrangian dual of problem (37) is problem (31), and 


vice-versa. 


d* = 


Strong duality holds between these two problems, i.e., 
Moreover, if the optimal solution X* of (37) has rank one, then s* = /* 
and hence d* = /*. 


Proof. The fact that the SDPs 


and pl|) are related by duality can be 

since these 


found in standard textbooks (e.g. [7, Example 5.13]); moreover 
are convex programs, under constraint qualification, the duality gap is zero, 
i.e., d* = s*. To prove that rank(X*) = 1 =>■ s* = d* = /*, we observe that 
(i) Tr WX* = s* < f* since (37) is a relaxation of (35). However, when 
rank(X*) = 1, X* is feasible for problem (37), hence, by optimality of /*, 
it holds (ii) /* < f(X*) = Tr WX*. Combining (i) and (ii) we prove that, 
when rank(X*) = 1, then f* = s*, which also implies f* = d*. □ 

To the best of our knowledge this is the first time in which the SDP re¬ 
laxation has been proposed to solve PGO. For the rotation subproblem, SDP 
relaxations have been proposed in [ZD EH Eg. According to Proposition [8j 
one advantage of the SDP relaxation approach is that we can a-posteriori 
check if the duality (or, in this case, the relaxation) gap is zero, from the 
optimal solution X*. Indeed, if one solves ( |37[ ) and finds that the optimal 
X* has rank one, then we actually solved ( |28[ ), hence the relaxation gap is 
zero. Moreover, in this case, from spectral decomposition of X* we can get 
a vector x* such that X* = (x*)(x*)*, and this vector is an optimal solution 
to the primal problem. 

In the following section we derive similar a-posteriori conditions for the 
dual problem (31). These conditions enable the computation of a primal 
optimal solution. Moreover, they allow discussing the uniqueness of such 
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solution. Furthermore, we prove that in special cases we can provide a- 
priori conditions that guarantee that the duality gap is zero. 


4.3 Analysis of the dual problem 


In this section we provide conditions under which the duality gap is zero. 
These conditions depend on the spectrum of W(A*), which arises from the 
solution of ©• We refer to VF(A*) as the penalized pose graph matrix. A 
first proposition establishes that © attains an optimal solution. 

Proposition 9 The optimal value d* in © is attained at a finite A*. 
Moreover, the penalized pose graph matrix W(A*) has an eigenvalue in 0. 


Proof. Since VF(A) y 0 implies that the diagonal entries are nonnegative, 
the feasible set of (31) is contained in the set {A : Wu — Aj > 0, i = 
1,... ,2 n — 1} (recall that Wu are reals according to Remark J2|. On the 
other hand, A i = Chn-i is feasible and all points in the set {A : A* > 0 
yield an objective that is at least as good as the objective value at Aj. 
Therefore, the problem is equivalent to rriax^ JA A* subject to the original 
constraint, plus a box constraint on A G {0 < A,; < W tl . i = 1,..., n}. Thus 
we maximize a linear function over a compact set, hence a finite optimal 
solution A* must be attained. 

Now let us prove that W{X k ) has an eigenvalue in zero. Assume by 
contradiction that W(A*) y 0. From the Schur complement rule we know: 


W(A*) 


L y 0 

Q(A*) -SM-'S y 0 


(38) 


The condition L y 0 is always satisfied for a connected graph, since L = 
A T A, and the anchored incidence matrix A, obtained by removing a node 
from the original incidence matrix, is always full-rank for connected graphs m 
Section 19.3]. Therefore, our assumption W{ A*) >- 0 implies that 


Q{ A*) - = Q- S*L~ l S - diag(A*) >- 0 


(39) 


Now, let 


e = A min (Q(A*) - SM-'S) > 0. 
which is positive by the assumption W (A*) >- 0. Consider A = A* + el, then 
Q(A) - SM-'S = Q( A) - SM-'S - el y 0 , 


thus A is dual feasible, and Aj > Yli A*, which would contradict optimal¬ 
ity of A*. We thus proved that Q(X*) must have a zero eigenvalue. □ 
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Proposition 10 (No duality gap) If the zero eigenvalue of the penalized 
pose graph matrix VE(A*) is simple then the duality gap is zero, i.e., d* = f*. 


Proof. We have already observed in Proposition [8] that (37) is the dual 
problem of (31), therefore, we can interpret X as a Lagrange multiplier 
for the constraint W( A) >z 0. If we consider the optimal solutions X* and 
A* of (37) and plj ), respectively, the complementary slackness condition 
ensures that Tr (W(A*)X*) = 0 (see fT], Example 5.13]). Let us parametrize 
X* y 0 as 

2n—1 

X* = invivt, 

2 = 1 


where 0 < p,\ < /12 < ■ • • < H 2 n -1 are the eigenvalues of X, and v, form a 
unitary set of eigenvectors. Then, the complementary slackness condition 
becomes 


Tr (W{\*)X*) 


/ 2n—1 

Tr W{ A*) pHVtv* 


i= 1 


2n—1 


mvi 


^ iTr 

2=1 
In -1 

^ m v* w{x*)vi = o. 


2=1 


Since W(A*) ^ 0, the above quantity is zero at a nonzero X * (X* cannot be 
zero since it needs to satisfy the constraints Xu = 1) if and only if m = 0 
for i = m + 1,.. ., 2n — 1, and W (A *)vi = 0 for i = 1,..., m, where m is the 
multiplicity of 0 as an eigenvalue of IE(A*). Hence X * has the form 

m 

x* = J2mwl ( 40 ) 

2=1 


where v,\, i = l,...,m, form a unitary basis of the null-space of VP(A*). 
Now, if m = 1, then the solution X * to problem (37) has rank one, but 
according to Proposition [8] this implies d * = /*, proving the claim. □ 


In the following we say that VE(A*) satisfies the single zero eigenvalue 
property (SZEP) if its zero eigenvalue is simple. The following corollary 
provides a more explicit relation between the solution of the primal and the 
dual problem when W(A*) satisfies the SZEP. 
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Corollary 1 (SZEP => x* £ T(A*)) If the zero eigenvalue ofW{ A*) is sim¬ 
ple, then the set X(X*) contains a primal optimal solution. Moreover, the 
primal optimal solution is unique, up to an arbitrary rotation. 


Proof. Let x* be a primal optimal solution, and let /* = (x*)*W(x*) be 


the corresponding optimal value. From Proposition 10 we know that the 
SZEP implies that the duality gap is zero, i.e., d* = /*, hence 


* = (x*)*W(x*). 


(41) 


i=l 


Since x* is a solution of the primal, it must be feasible, hence |ai *| 2 = 1, 
i = n,... ,2n — 1. Therefore, the following equalities holds: 


E A ^ = I>^n+i-ii 2 = (z*r 


i =1 


i =1 


o o 

0 diag(A*) 


(**) 


(42) 


Plugging (|42|) back into (|41|): 

(**) 


W — 


0 0 
0 diag(A*) 


(x*) = 0 47 (x*)*W(A*)(x*) = 0 (43) 


which proves that x * belongs to the null space of VF(A*), which coincides 
with our definition of T(A*) in (33), proving the first claim. 


Let us prove the second claim. From the first claim we know that the 
SZEP implies that any primal optimal solution is in T(A*). Moreover, when 
W{ A*) has a single eigenvalue in zero, then T(A*) = Kernel(W(A*)) is 1- 
dimensional and can be written as X(X*) = { 7 s* : 7 £ C}, or, using the 
polar form for 7 : 

X(X*) = {ge 3 ^ x* : g, y G M} (44) 


From (44) it’s easy to see that any i) ^ 1 would alter the norm of x* 


leading to a solution that it’s not primal feasible. On the other hand, any 
e^x* belongs to ff(A*), and it’s primal feasible (le^x*! = |x*|), hence by 
Theorem [lj any is primal optimal. We conclude the proof by noting 

that the multiplication by corresponds to a global rotation of the pose 
estimate x*: this can be easily understood from the relation (18). □ 


Proposition 10 provides an a-posteriori condition on the duality gap, 


that requires solving the dual problem; while Section [ 6 ] will show that this 
condition is very useful in practice, it is also interesting to devise a-priori 
conditions, that can be assessed from the pose graph matrix W, without 
solving the dual problem. A first step in this direction is the following 
proposition. 
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Proposition 11 (Strong duality in trees and balanced pose graphs) 

Strong duality holds for any balanced pose graph optimization problem, and 
for any pose graph whose underlying graph is a tree. 

Proof. Balanced pose graphs and trees have in common the fact that 
they attain /* = 0 (Propositions [l]{2]). By weak duality we know that d * < 0. 
However, A = 0 n is feasible (as W 0) and attains d( A) = 0, hence A = 0 n 
is feasible and dual optimal, proving d* = /*. □ 


5 Algorithms 


In this section we exploit the results presented so far to devise an algorithm 
to solve PGO. The idea is to solve the dual problem, and use A* and W (A*) to 
compute a solution for the prim al PGO problem. We split the presentation 
into two sections: Secti on |5.l| discusses the case in which W( A*) satisfies 
the SZEP, while Section |5.2| discusses the case in which W{ A*) has multiple 
eigenvalues in zero. This distinction is important as in the former case 
(which is the most common in practice) we can compute a provably optimal 
solution for PGO, while in the latter case our algorithm returns an estimate 


that is not necessarily optimal. Finally, in Section 5.3 
algorithm and present the corresponding pseudocode. 


we summarize our 


5.1 Case 1: W( A*) satisfies the SZEP 

According to Corollary [l] if W( A*) has a single zero eigenvalue, then the 
optimal solution of the primal problem x* is in A(A*), where A(A*) coincides 
with the null space of W(A*), as per (33). Moreover, this null space is 1- 
dimensional, hence it can be written explicitly as: 


X(X*) = Kernel(W(A*)) = {v e C 


2n—1 


v = yx*}, 


(45) 


which means that any vector in the null space is a scalar multiple of the 
primal optimal solution x*. This observation suggests a computational ap¬ 
proach to compute x*. We can first compute an eigenvector v corresponding 
to the single zero eigenvalue of W (A*) (this is a vector in the null space of 
VP(A*)). Then, since x* must be primal feasible (i.e., \x n \ = ... = |x 2 n-i| = 
1), we compute a suitable scalar 7 that makes primal feasible. This scalar 
is clearly 7 = 1 4)^| = ... = |u 2 n-i| (we essentially need to normalize the norm 
of the last n entries of v). The existence of a suitable 7 , and hence the fact 
that \v n \ = ... = |b 2 n-i| > 0, is guaranteed by Corollary [I] As a result we 
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get the optimal solution x* = -v. The pseudocode of our approach is given 


in Algorithm [lj and further discussed in Section 5.3 


5.2 Case 2: IE(A*) does not satisfy the SZEP 

Currently we are not able to compute a guaranteed optimal solution for 
PGO, when W"(A*) has multiple eigenvalues in zero. Nevertheless, it is 
interesting to exploit the solution of the dual problem for finding a (possibly 
suboptimal) estimate, which can be used, for instance, as initial guess for 
an iterative technique. 

Eigenvector method. One idea to compute a suboptimal solution from 
the dual problem is to follow the same approach of Section 5.1 we compute 
an eigenvector of IT(A*), corresponding to one of the zero eigenvalues, and 
we normalize it to make it feasible. In this case, we are not guaranteed that 
\v n \ = ... = |u 2 n-i| > 0 (as in the previous section), hence the normalization 
has to be done component-wise, for each of the last n entries of v. In 
the following, we consider an alternative approach, which we have seen to 
perform better in practice (see experiments in Section [ 6 ]). 

Null space method. This approach is based on the insight of Theo¬ 
rem^ if there is a primal feasible x £ A(A*), then x must be primal optimal. 
Therefore we look for a vector x £ A(A*) that is “close” to the feasible set. 
According to ( p3| ), A(A*) coincides with the null space of IT(A*). Let us 
denote with V £ C^ 2n ~ 1 ^ xq a basis of the null space of W(\*), where q is 
the number of zero eigenvalues of IE(A*)j^] Any vector x in the null space 
of IT(A*) can be written as x = Vz, for some vector S £ CL Therefore 
we propose to compute a possibly suboptimal estimate x = Vz*, where z* 
solves the following optimization problem: 


2n—1 


max ) real (Viz) + imag(V) 
i =1 

s.t.: |E ^| 2 < 1 , 


i.Z 


(46) 


i = n ,..., 2 n — 1 


where Vi denotes the z-th row of V, and real(-) and imag(-) return the real 
and the imaginary part of a complex number, respectively. For an intuitive 


explanation of problem (46), we notice that the feasible set of the primal 
problem (28) is described by \xi\ 2 = 1, for i = n,.. ., 2n—1. In problem (46) 
we relax the equality constraints to convex inequality constraints \xi\ 2 < 1, 
for i = n ,..., 2n — 1; these can be written as \Vz\ 2 < 1, recalling that we 


can be computed from singular value decomposition of W/(A*). 
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are searching in the null space of W (A*), which is spanned by Vz. Then, the 
objective function in (46) encourages “large” elements Viz, hence pushing 
the inequality |Vj5| 2 < 1 to be tight. While other metrics can force large 
entries Vz, we preferred the linear metric (|46|) to preserve convexity. 


Note that x = Vz*, in general, is neither optimal nor feasible for our 


PGO problem (28), hence we need to normalize it to get a feasible estimate. 
The experimental section provides empirical evidence that, despite being 
heuristic in nature, this method performs well in practice, outperforming — 
among the others- the eigenvector method presented earlier in this section. 


5.3 Pseudocode and implementation details 

The pseudocode of our algorithm is given in Algorithm [lj The first step is 
to solve the dual problem, and check the a-posteriori condition of Proposi¬ 
tion 10 If the SZEP is satisfied, then we can compute the optimal solution 
by scaling the eigenvector of W (A*) corresponding to the zero eigenvalue fii. 


This is the case described in Section [5. 1| and is the most relevant in practice, 
since the vast majority of robotics problems falls in this case. 

The “else” condition corresponds to the case in which W{ A*) has multiple 
eigenval ue i n zero. The pseudocode implements the null space approach of 
The algorithm computes a basis for the null space of W(A*) 


5.2 


Section 

and solves (46) to find a vector belonging to the null space (i.e, in the form 


x = Vz) that is close to the feasible set. Since such vector is not guaranteed 
to be primal feasible (and it is not in general), the algorithm normalizes the 
last n entries of x* = Vz *, so to satisfy the unit norm constraints in (28). 


Besides returning the estimate x *, the algorithm also provides an optimality 
certificate when W (A*) has a single eigenvalue in zero. 
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input : Complex PGO matrix W 

output: Primal solution x * and optimality certificate isOpt 


solve the dual problem (31) and get A* ; 

if W( A*) has a single eigenvalue pi at zero then 

compute the eigenvector v of VF(A*) corresponding to pp, 
compute x* = ^v, where 7 = | v 3 |, for any j E {n,..., 2n — 1} ; 
set isOpt = true; 
else 

compute a basis V for the null space of bP(A*) using SVD; 
compute z* by solving the convex problem ( |46| ); 
set x * = Vz* and normalize |xj| to 1, for all i = n,..., 2n — 1; 
set isOpt = unknown; 
end 

return (x*, isOpt) 


Algorithm 1: Solving PGO using Lagrangian duality. 


6 Numerical Analysis and Discussion 

The objective of this section is four-fold. First, we validate our theoret¬ 
ical derivation, providing experimental evidence that supports the claims. 
Second, we show that the duality gap is zero in a vast amount or practical 
problems. Third, we confirm the effectiveness of Algorithm [l] to solve PGO. 
Fourth, we provide toy examples in which the duality gap is greater than 
zero, hoping that this can stimulate further investigation towards a-priori 
conditions that ensure zero duality gap. 


Simulation setup. For each run we generate a random graph with n = 10 
nodes, unless specified otherwise. We draw the position of each pose by a 
uniform distribution in a 10m x 10m square. Similarly, ground truth node 
orientations are randomly selected in (— n, +7t]. Then we create set of edges 
defining a spanning path of the graph (these are usually called odometric 
edges)-, moreover, we add further edges to the edge set, by connecting ran¬ 
dom pairs of nodes with probability P c = 0.1 (these are usually called loop 
closures ). From the randomly selected true poses, and for each edge (i,j) in 
the edge set, we generate the relative pose measurement using the following 
model: 


A ij = Pj (Pj - Pi) + e A , 

Rij = Rj Rj R{€r), 
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e A ~ N(0 2 ,a 2 A ) 
e R ~ N(0,a%) 


(47) 










Figure 4: Smallest eigenvalue of W (in logarithmic scale) for different levels 
of (a) rotation noise (std: (Jr)-, and (b) translation noise (std: cta). The 
figure show the minimum observed value over 100 Monte Carlo runs, for non¬ 
tree graphs. The minimum eigenvalue is zero only if the graph is balanced. 


where ca E M 2 and 6r E M are zero-mean Normally distributed random 
variables, with standard deviation cta and or, respectively, and R(eR) is 
a random planar rotation of an angle 6r. Unless specified otherwise, all 
statistics are computed over 100 runs. 

Spectrum of W. In Proposition [HJ we showed that the complex anchored 
pose graph matrix W has at most one eigenvalue in zero, and the zero 
eigenvalue only appears when the pose graph is balanced or is a tree. 

Fig. [ija) reports the value of the smallest eigenvalue of W (in log scale) 
for different or, with fixed cta = 0m. When also or is zero, the pose graph 
is balanced, hence the smallest eigenvalue of W is (numerically) zero. For 
increasing levels of noise, the smallest eigenvalue increases and stays away 
from zero. Similarly, Fig. Qb) reports the value of the smallest observed 
eigenvalue of W (in log scale) for different cta, with fixed or = Or ad. 


Duality gap is zero in many cases. This section shows that for the 
levels of measurement noise of practical interest, the matrix IU(A*) satis¬ 
fies the Single Eigenvalue Property (SZEP), hence the duality gap is zero 
(Proposition |10[). We consider the same measurement model of Eq. (47), 
and we analyze the percentage of tests in which IU(A*) satisfies the SZEP. 

Fig . [5|a) shows the percentage of the experiments in which the penalized 
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Figure 5: Percentage of problems in which W{ A*) satisfied the SZEP prop¬ 
erty, for different (a) rotation measurement noise or, (b) translation mea¬ 
surement noises a a, (c) loop closure probability P c , (d) number of nodes n. 


pose graph matrix VF(A*) has a single zero eigenvalue, for different values 
of rotation noise or, and keeping fixed the translation noise to a a = 0.1m 
(this is a typical value in mobile robotics applications). For or < 0.5rad, 
VF(A*) satisfies the SZEP in all tests. This means that in this range of 
operation, Algorithm [l] is guaranteed to compute a globally-optimal solution 
for PGO. For or = lrad, the percentage of successful experiments drops, 
while still remaining larger than 90%. Note that or = lrad is a very large 
rotation noise (in robotics, typically or < 0.3rad m, and it is not far 
from the case in which rotation measurements are uninformative (uniformly 
distributed in (—7r, +7r]). To push our evaluation further we also tested this 
extreme case. When rotation noise is uniformly distributed in (—7 t,+7t], 
we obtained a percentage of successful tests (single zero eigenvalue) of 69%, 
which confirms that the number of cases in which we can compute a globally 
optimal solution drops gracefully when increasing the noise levels. 

Fig. [H](b) shows the percentage of the experiments in which TT(A*) has 
a single zero eigenvalue, for different values of translation noise a a, and 
keeping fixed the rotation noise to or = O.lrad. Also in this case, for 
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practical noise regimes, our approach can compute a global solution in all 
cases. The percentage of successful tests drops to 98% when the translation 
noise has standard deviation lm. We also tested the case of uniform noise 
on translation measurements. When we draw the measurement noise from 
a uniform distribution in [—5,5] 2 (recall that the poses are deployed in a 
10 x 10 square), the percentage of successful experiments is 68%. 

We also tested the percentage of experiments satisfying the SZEP for 
different levels of connectivity of the graph, controlled by the parameter P c . 
We observed 100% successful experiments, independently on the choice of 
P c , for gr = a a = 0.1 and gr = a a = 0.5. A more interesting case if 
shown in Fig. [5](c) and corresponds to the case gr = g\ = 1- The SZEP 
is always satisfied for P c = 0: this is natural as P c = 0 always produces 
trees, for which we are guaranteed to satisfy the SZEP (Proposition 0. 
For P c = 0.2 the SZEP fails in few runs. Finally, increasing the connectivity 
beyond P c = 0.4 re-establishes 100% of successful tests. This would suggest 
that the connectivity level of the graph influences the duality gap, and better 
connected graphs have more changes to have zero duality gap. 

Finally, we tested the percentage of experiments satisfying the SZEP 
for different number of nodes n. We tested the following number of nodes: 
n = {10,20,30,40,50}. For gr = <ta = 0.1 and gr = ga = 0.5 the SZEP 
was satisfied in 100% of the tests, and we omit the results for brevity. The 
more challenging case gr = ga = 1 is shown in Fig. [5](d). The percentage of 
successful tests increases for larger number of poses. We remark that current 
SDP solvers do not scale well to large problems, hence a Monte Carlo analysis 
over larger problems becomes prohibitive. We refer the reader to [11] for 
single-run experiments on larger PGO problems, which confirm that the 
duality gap is zero in problems arising in real-world robotics applications. 

Performance of Algorithm [l] In this section we show that Algorithm [l] 
provides an effective solution for PGO. When W{ A*) satisfies the SZEP, 
the algorithm is provably optimal, and it enables to solve problems that 
are already challenging for iterative solvers. When the VE(A*) does not 
satisfy the SZEP, we show that the proposed approach, while not providing 
performance guarantees, largely outperforms competitors. 

Case 1: W{\*) satisfies the SZEP. When W(A*) satisfies the SZEP, Al¬ 
gorithm [l] is guaranteed to produce a globally optimal solution. However, 
one may argue that in the regime of operation in which the SZEP holds, PGO 
problem instances are sufficiently “easy” that commonly used iterative tech¬ 
niques also perform well. In this paragraph we briefly show that the SZEP 
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Figure 6: Statistics on tests in which the SZEP is satisfied: the figure 
reports the gap between the optimal objective /* attained by Algorithm [T 
and the objective fcN attained by a Gauss-Newton method initialized at the 
true poses. We simulate different combinations of noise (see x-axis labels), 
keeping fixed n = 10 and P c = 0.1. The label “unif.” denotes uniform noise 
for rotations (in (—7r,+vr]) or translations (in [—5,+5]). 


is satisfied in many instances that are hard to solve. For this purpose, we fo¬ 
cus on the most challenging cases we discussed so far, i.e., problem instances 
with large rotation and translation noise. Then we consider the problems 
in which the SZEP is satisfied and we compare the solution of Algorithm [lj 
which is proven to attain /*, versus the solution of a Gauss-Newton method 
initialized at the true poses. Ground truth poses are an ideal initial guess 
(which is unfortunately available only in simulation): intuitively, the global 
minimum of the cost should be close to the ground truth poses (this is one 
of the motivations for maximum likelihood estimation). Fig. [b] shows the 
gap between the objective attained by the Gauss-Newton method (denoted 
as /gn) and the optimal objective obtained from Algorithm [lj The figure 
confirms that our algorithm provides a guaranteed optimal solution in a 
regime that is already challenging, and in which iterative approaches may 
fail to converge even from a good initialization. 

Case 2: W( A*) does not satisfy the SZEP. In this case, Algorithm [I] 
computes an estimate, according to the null space approach proposed in 
Section |5.2| we denote this approach with the label NS. To evaluate the 
performance of the proposed approach, we considered 100 instances in which 
the SZEP was not satisfied and we compared our approach against the 
following methods: a Gauss-Newton method initialized at the ground truth 


poses (GN), the eigenvector method described at the beginning of Section 5.2 
(Eig), and the SDP relaxation of Section 4.2 (SDP 
we compute the solution X * of the relaxed problem (37) 


For the SDP approach, 
If X * has rank 
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Figure 7: Statistics on tests in which the SZEP is not satisfied: (al)-(a4): 
Comparison of different PGO solvers for different levels of noise. The com¬ 
pared approaches are: a Gauss-Newton method initialized at the ground 
truth poses (GN), the proposed null space approach (NS), the eigenvector 
method (Eig), the SDP relaxation (SDP). (bl)-(b4): Comparison of the 
techniques GN, NS. Eig, SDP, refined with a Gauss-Newton method, and an 
alternative approach which solves for rotations first (EigR). 


larger than 1, we find the closest rank-1 matrix Wank-i from singular value 
decomposition |28|. Then we factorize Wa nk -1 as W an k-i = xx* (x can be 
computed via Cholesky factorization of W ank -i [7H]b We report the results 
of our comparison in the first row of Fig. [Tj where we show for different noise 
setups (sub-figures (al) to (a4)), the cost of the estimate produced by the 
four approaches. The proposed null space approach (NS) largely outperforms 
the Eig and the SDP approaches, and has comparable performance with an 
“oracle” GN approach which knows the ground truth poses. 

One may also compare the performance of the approaches NS, Eig, SDP 
after refining the corresponding estimates with a Gauss-Newton method, 
which tops off residual errors. The cost obtained by the different techniques, 
with the Gauss-Newton refinement, are shown in the second row of Fig. [7j 
For this case we also added one more initialization technique in the com¬ 
parison: we consider an approach that solves for rotations first, using the 
eigenvalue method in EQj, and then applies the Gauss-Newton method from 
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Figure 8: (a) Toy example of chain pose graph in which the SZEP fails. In 

each plot we also report the four smallest eigenvalues of the penalized pose 
graph matrix W{ A*) for the corresponding PGO problem. Removing a node 
from the original graph may change the duality properties of the graph. In 
(b), (c), (d), (e), (f) we remove nodes 1, 2, 3, 4, 5, respectively. Removing 
any node, except node 3, leads to a graph that satisfied the SZEP. 


the rotation guess|^] Fig. [7^bl) to Fig. [7^b4) show less differences (in aver¬ 
age) among the techniques, as in most cases the Gauss-Newton refinement 
is able to converge starting from all the compared initializations. However, 
for the techniques Eig, SDP, and EigR we see many red sample points, which 
denote cases in which the error is larger than the 75th percentile; these are 
the cases in which the techniques failed to converge and produced a large 
cost. On the other hand, the proposed NS approach is less prone to converge 
to a bad minimum (fewer and lower red samples). 

Chain graph counterexample and discussion. In this section we con¬ 
sider a simple graph topology: the chain graph. A chain graph is a graph 
with edges (1, 2), (2, 3),..., (n — 1, n), (n, 1). Removing the last edge we ob¬ 
tain a tree (or, more specifically, a path), for which the SZEP is always satis¬ 
fied. Therefore the question is: is the SZEP always satisfied in PGO whose 

5 This was not included in the first row of Fig. [7] as it does not provide a guess for the 
positions of the nodes. 
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Figure 9: Second eigenvalue of the matrix W{ A*) for different variations 
of the toy graph of Fig. [8^ a). Each variation is obtained by scaling the 
translation measurements of the original graph by the amount specified on 
the x-axis of this figure. When the scale of the measurement is < 0.4 the 
second eigenvalue of W{ A*) is larger than zero, hence the SZEP is satisfied. 


underlying graph is a chain? The answer, unfortunately, is no. Fig. [8j^a) 
provides an example of a very simple chain graph with 5 nodes that fails to 
meet the SZEP property. The figure reports the 4 smallest eigenvalues of 
VF(A*) (hi, ... ,/m), and the first two are numerically zero. 

If the chain graph were balanced, Proposition says that the SZEP 
needs to be satisfied. Therefore, one may argue that failure to meet the 
SZEP depends on the amount of error accumulated along the loop in the 
graph. Surprisingly, also this intuition fails. In Fig. j8jb-f) we show the pose 
graphs obtained by removing a single node from the pose graph in Fig. [8^a). 
When removing a node, say k, we introduce a relative measurement between 
nodes k — 1 and k + 1, that is equal to the composition of the relative 
measurements associated to the edges (k — 1, k) and (k, k + 1) in the original 
graph. By constructions, the resulting graphs have the same accumulated 
errors (along each loop) as the original graph. However, interestingly, they 
do not necessarily share the same duality properties of the original graph. 
The graphs obtained by removing nodes 1,2,4, 5 (shown in figures b,c,e,f, 
respectively), in fact, satisfy the SZEP. On the other hand, the graph in 
Fig. |8](c) still has 2 eigenvalues in zero. The data to reproduce these toy 
examples are reported in Appendix |8.8[ 

We conclude with a test showing that the SZEP is not only dictated by 
the underlying rotation subproblem but also depends heavily on the trans¬ 
lation part of the optimization problem. To show this we consider variations 
of the PGO problem in Fig. [8](a), in which we “scale” all translation mea¬ 
surements by a constant factor. When the scale factor is smaller than one 
we obtain a PGO problem in which nodes are closer to each other; for scale 
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> 1 we obtain larger inter-nodal measurements; the scale equal to 1 coin¬ 
cides with the original problem. Figjiljshows the second eigenvalue of W (A*) 
for different scaling of the original graphs. Scaling down the measurements 
in the graph of Fig. [8^a) can re-establish the SZEP. Interestingly, this is in 
agreement with the convergence analysis of CD!, which shows that the basin 
of convergence becomes larger when scaling down the inter-nodal distances. 

7 Conclusion 

We show that the application of Lagrangian duality in PGO provides an 
appealing approach to compute a globally optimal solution. More specifi¬ 
cally, we propose four contributions. First, we rephrase PGO as a problem 
in complex variables. This allows drawing connection with the recent liter¬ 
ature on unit gain graphs, and enables results on the spectrum of the pose 
graph matrix. Second, we formulate the Lagrangian dual problem and we 
analyze the relations between the primal and the dual solutions. Our key 
result proves that the duality gap is connected to the number of eigenvalues 
of the penalized pose graph matrix , which arises from the solution of the 
dual problem. In particular, if this matrix has a single eigenvalue in zero 
(SZEP), then (i) the duality gap is zero, (ii) the primal PGO problem has 
a unique solution (up to an arbitrary roto-translation), and (iii) the primal 
solution can be computed by scaling an eigenvector of the penalized pose 
graph matrix. The third contribution is an algorithm that returns a guaran¬ 
teed optimal solution when the SZEP is satisfied, and (empirically) provides 
a very good estimate when the SZEP fails. Finally, we report numerical re¬ 
sults, that show that (i) the SZEP holds for noise levels of practical robotics 
applications, (ii) the proposed algorithm outperforms several existing ap¬ 
proaches, (iii) the satisfaction of the SZEP depends on multiple factors, 
including graph connectivity, number of poses, and measurement noise. 

8 Appendix 

8.1 Proof of Proposition [lj Zero Cost in Trees 

We prove Proposition |T] by inspection, providing a procedure to build an 
estimate that annihilates every summand in ([8]). The procedure is as follows: 

1. Select a root node, say the first node (p«,r,), with i = 1, and set it to 
the origin, i.e., p % = O2, r* = [1 0] T (compare with ([7]) for 0. t = 0); 
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2. For each neighbor j of the root i, if j is an outgoing neighbor, set 
rj = RijVi , and pj = Pi + D^ri, otherwise set rj = Rj^i, and pj = 

Pi + DjiVj ; 

3. Repeat point [2] for the unknown neighbors of every node that has been 
computed so far, and continue until all poses have been computed. 

Let us now show that this procedure produces a set of poses that annihi¬ 
lates the objective in Q. According to the procedure, we set the first node 
to the origin: p\ = O 2 , r\ = [1 0] T ; then, before moving to the second step 
of the procedure, we rearrange the terms in ([8]): we separate the edges into 
two sets £ = £\ U £\ , where £\ is the set of edges incident on node 1 (the 
root), and £\ are the remaining edges. Then the cost can be written as: 

f(p,r)= ^2 \\pj - Pi- D ij r i\\2+\\ r 3 - Rij r i\\l + 

(hi) e£i 

+ ^2 \\Pj - Pi- D ij r i\\l+\\ r i - R ij r i\\l ( 48 ) 

(i,j)e £1 

We can further split the set £\ into edges that have node 1 as a tail (i.e., 
edges in the form (1, j)) and edges that have node 1 as head (i.e., (j, 1)): 

f(p,r)= \\Pj ~ Pi ~ D ijfi \\2 + \\fj ~ RijriWl + 

(lj')j'6A/T ut 

+ ^2 Ibi -Pj - D ji r j\\l + lln - RjirjWl + 

(j'.i)je R'r 

+ \\Pj-Vi- D ij r i\\l + \\rj - RijTiWl (49) 

(i,j)e £1 

Now, we set each node j in the first two summands as prescribed in step 2 
of the procedure. By inspection one can verify that this choice annihilates 
the fist two summands and the cost becomes: 

f(p,r)= ^2 lb? ~Pi ~ D ii r i\\l + \\ r i ~ R ij r i\\l (50) 

Now we select a node k that has been computed at the previous step, but 
has some neighbor that is still unknown. As done previously, we split the 
set £\ into two disjoint subsets: £\ = £^ U £*., where the set £*. contains the 
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edges in £\ that are incident on k, and £k contains the remaining edges: 


f(p,r)= Y2 Ibj ~Pk ~ D kj r k \\l + \\rj - R kj r k \\l + 

{(MJe AT° ut }n£i 

+ Ibfc ~ Pj ~ DjkTj\\2 + \\rk — Rjk r j\\2 + 

{0»deArf}n£i 

T Y ( bj Pi Tbj T{ 11 2 T | Cj RijVi || 2 (51) 

(*j) effc 


Again, setting neighbors j as prescribed in step 2 of the procedure, anni¬ 
hilates the first two summands in (51). Repeating the same reasoning for 
all nodes that have been computed, but still have unknown neighbors, we 
can easily show that all terms in (51) become zero (the assumption of graph 
connectivity ensures that we can reach all nodes), proving the claim. 


8.2 Proof of Proposition [2} Zero Cost in Balanced Graphs 

Similarly to Appendix |8.1[ we prove Proposition [2] by showing that in bal¬ 
anced graphs one can always build a solution that attains zero cost. 

For the assumption of connectivity, we can find a spanning tree T of the 
graph, and split the terms in the cost function accordingly: 

f(p, r) = ^2 il Pj ~Pi~ D ij r i\\l + lb j ~ RijfiWl + 

(ij)er 

T b ( \\Pj P l T'i 112 T | Tj n 11 2 (52) 

(ij)er 


where T = £\T are the chords of the graph w.r.t. T. 
Then, using the procedure in Appendix 


8.1 we construct a solution 


{r*,p*} that attains zero cost for the measurements in the spanning tree 
T. Therefore, our claim only requires to demonstrate that the solution built 
from the spanning tree also annihilates the terms in T : 


/(P* 


r = 


Y2 \\p*j - Pi - D a r i 

(iJ)eT 


I 2 + 
12 + 


- Rijr, 


(53) 


To prove the claim, we consider one of the chords in T and we show that 
the cost at {r*,p*} is zero. The cost associated to a chord (i, j) E T is: 


M-pt-DijrtWi + Wr. 


- Rijr % 


* || 2 
2 


(54) 
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Now consider the unique path Vij in the spanning tree T that connects i to 
j, and number the nodes along this path as i, i + 1,..., j — 1, j. 

Let us start by analyzing the second summand in (54), which corresponds 
to the rotation measurements. According to the procedure in Appendix |8.1| 
to build the solution for 7~, we propagate the estimate from the root of the 
tree. Then it is easy to see that: 


Tj — /fj 1 j ' ' ' Ri+li+2Rii-\-Vi 


(55) 


where Ru+i is the rotation associated to the edge (z, i + 1), or its transpose 
if the edge is in the form (z + l,z) (i.e., it is traversed backwards along 
Vij). Now we notice that the assumption of balanced graph implies that the 
measurements compose to the identity along every cycle in the graph. Since 
the chord (i. j) and the path Vij form a cycle in the graph, it holds: 


Ri— 


■j-ij' 


R 




Rii- 1-1 — R', 


v 


Substituting (56) back into (55) we get: 


rj = Rij Ti 


(56) 

(57) 


which can be easily seen to annihilate the second summand in (54). 

Now we only need to demonstrate that also the first summand in (54) is 
zero. The procedure in Appendix |8 .1 1 leads to the following estimate for the 
position of node j : 

P]j = Pi + Ai+ir* + D i+u+2 r* +1 + ■ ■ ■ + ( 58 ) 

— Pi T Dii-\-lfi T Di-\-li-\-2Rii+lfi T ' ' ' T ^j—ljRj—2j—l ' ' ' ^1+11+2^11+1^ 
= Pi + {Dii+1 + Di+li+2Rii+l + ‘ ‘ ‘ + Dj~ljRj-2j-l ‘ ' ' Ri+li+2Rii+l) T* 

The assumption of balanced graph implies that position measurements com¬ 
pose to zero along every cycle, hence: 

1 + 7?jj+iAj_|_ij_|_2 + Ri+li+2Rii+l\+2i+3 + ' ' ' 

T Rj—2j—l ‘ ‘ ‘ Ri+ li+27?u+l ^j—lj (59) 

or equivalently: 

Da+l + Di+li+2Rii+l + ' ' ' 

T Rj 1 j Rj 2] 1 ' ‘ ‘ Ri+li+2Rii+l (60) 


\j — 


Dij — 


Substituting (60) back into (58) we obtain: 

Pj = P* + Ayr* 


which annihilates the first summand in (54), concluding the proof. 
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8.3 Proof of Proposition [3} properties of W 


Let us prove that W has (at least) two eigenvalues in zero. We already 
observed that the top-left block of W is C = C <g> I 2 , where C is the Laplacian 
matrix of the graph underlying the PGO problem. The Laplacian C of 
a connected graph has a single eigenvalue in zero, and the corresponding 
eigenvector is \ n (see, e.g., [18, Sections 1.2-1.3]), i.e., £• l n = 0. Using this 
property, it is easy to show that the matrix N = [0j l)[] T <8> h is in the 
nullspace of W, i.e., WN = 0. Since N has rank 2, this implies that the 
nullspace of W has at least dimension 2, which proves the first claim. 

Let us now prove that the matrix W is composed by 2 x 2 blocks [Wjij. 
with [W]ij 6 aSO( 2), Vi, j = 1,..., 2n, and [VV]^ = anR with an > 0. We 
prove this by direct inspection of the blocks of W. Given the structure of 
W in (14), the claim reduces to proving that the matrices £, Q, and A T D 
are composed by 2 x 2 blocks in aSO( 2), and the diagonal blocks of C and 
Q are multiples of the identity matrix. To this end, we start by observing 
that L = L ® I 2 , hence all blocks in C are multiples of the 2x2 identity 
matrix, which also implies that they belong to aSO( 2). Consider next the 
matrix Q = D T D + U T U. From the definition of D it follows that D T D is 
zero everywhere, except the 2x2 diagonal blocks: 


[D T D] U = ^ i = 1,...,n. (61) 

je W out 

Similarly, from simple matrix manipulation we obtain the following block 
structure of U T U: 


[U T U]n = dih, i = l,...,n; 
[U T U]ij = — Rij , (hj) £ £'■> 
[U T U}ij = —Rji, 

[U T U]ij = 02 x 2 , otherwise. 


(62) 


where di 
and (62) 


is the degree (number of neighbours) of node i. 
we get the following structure for Q: 


Combining (61) 


[Q]n = kh, 

— Riji 

[Q\ij = —Rji, 
[Q]ij = 02x2, 


i = 1,..., n; 

(i,j) e £; 

(. 3 , i) e £; 

otherwise. 


(63) 
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where we defined /% = dj + X^jeA/' out Clearly, Q has blocks in aSO( 2) 

and the diagonal blocks are nonnegative multiples of /o. 

Now, it only remains to inspect the structure of A T D. The matrix AJ D 
has the following structure: 

[A D\a = yCg a/- out Dij , i = 1,... ,iv, 

[A T Djij =-Dji, (j,i)e£; 

[A T D]ij = 02x2; otherwise. (64) 

Note that X^jeA/' out is the sum of matrices in aSO{ 2), hence it also 

belongs to aSO{ 2). Therefore, also all blocks of A T D are in a50(2), thus 
concluding the proof. 


8.4 Proof of Proposition [5} Cost in the Complex Domain 

Let us prove the equivalence between the complex cost and its real counter¬ 
part, as stated in Proposition [5j 

We first observe that the dot product between two 2-vectors xi,X 2 G R 2 , 
can be written in terms of their complex representation x\ = x^, and X 2 = 
# 2 > as follows: 

___ 'v ^'T n -\- nr -i nr * 

(65) 


T XXX 2 +X\X% 

x l x 2 = ---- 


Moreover, we know that the action of a matrix Z E aSO( 2) can be 


written as the product of complex numbers, see (18). 
Combining (65) and (18) we get: 

T „ X\ Z X2 + X\ Z* X% 

Xx Zx 2 ~ - ~ - 


( 66 ) 


where z = Z v . Furthermore, when Z is multiple of the identity matrix, it 
easy to see that 2 = Z v is actually a real number, and Eq. (66) becomes: 


xl Zx i ~ x\ z xi 


(67) 


x T Wx in complex form. Since W is symmetric, the product becomes: 


With the machinery introduced so far, we are ready to rewrite the cost 


2n-l 


x T Wx = ^2 


i=l 


2n—l 


xj [W]uXi + ^2 2 xj [Wjijxj 
j=i +1 


( 68 ) 
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Using the fact that \W]a is a multiple of the identity matrix, W tl = [W]~ 6 
M, and using ([67|) we conclude xj [W ]uXi = x*Wax t . Moreover, defining 


Wij = \W]ij y (these will be complex numbers, in general), and using (66), 
eq. ([681) becomes: 


2n—l 


x T Wx = 


E 

2=1 

2n—l 

E 

2=1 


2n—l 


xlWaXi + Y, (ZiWijxj + XiWifx* 
j=i +1 


tWiiXi + Y^iWija 

j¥=i 


= x*Wx 


(69) 


where we completed the lower triangular part of W as Wp = Wij*. 


8.5 Proof of Proposition Zero Eigenvalues in W 

Let us denote with Nq the number of zero eigenvalues of the pose graph 
matrix W. Nq can be written in terms of the dimension of the matrix 
(W G c(2n-i)x(2n-i)) anc j £} ie ran k G f the matrix: 


Nq = (2 n — 1) — rank(TU) 


(70) 


Now, recalling the factorization of W given in (25), we note that: 


rank(lU) = rank f 


A D 
0 U 


= rank(^4) + rank(P) 


(71) 


where the second relation follows from the upper triangular structure of the 
matrix. Now, we know from m Section 19.3] that the anchored incidence 
matrix A, obtained by removing a row from the the incidence matrix of a 
connected graph, is full rank: 


rank(A) = n — 1 (72) 

Therefore: 

Nq = n — rank(C7) (73) 

Now, since we recognized that U is the complex incidence matrix of a unit 
gain graph (Lemma [T]), we can use the result of Lemma 2.3 in [60] , which 
says that: 

rank(CZ) = n — b, (74) 

where b is the number of connected components in the graph that are bal¬ 
anced. Since we are working on a connected graph (Assumption [I]), b can be 
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either one (balanced graph or tree), or zero otherwise. Using (73) and (74), 
we obtain Nq = b , which implies that that No = 1 for balanced graphs or 
trees, or Nq = 0, otherwise. 

8.6 Proof of Proposition [7| Spectrum of Complex and Real 
Pose Graph Matrices 

Recall that any Hermitian matrix has real eigenvalues, and possibly com¬ 
plex eigenvectors. Let /i 6 K be an eigenvalue of W, associated with an 
eigenvector vGC 


2n —i 


i.e., 


Wv = fj,v 


From equation (75) we have, for i = 1,..., 2n — 1, 

2n —1 2n— 1 


y, Wijvj = y [W]ijvj = fiVi 

3=1 3=1 


(75) 


(76) 


where Vi is such that v / = v *. Since eq. (76) holds for all i = 1,..., 2n — 1, 
it can be written in compact form as: 


Wv = [xv 


(77) 


hence v is an eigenvector of the real anchored pose graph matrix W, asso¬ 
ciated with the eigenvalue //. This proves that any eigenvalue of W is also 
an eigenvalue of W. 

To prove that the eigenvalue is actually repeated twice in W, consider 
now equation (75) and multiply both members by the complex number e-U: 


~ 1 7T 7T 

Wvv 2 = uve 1 ? 

For i = 1,..., 2n — 1, we have: 

2n— 1 2n— 1 

y Wij*Vje > z = /uhje^ y [W]ijwj = fj,Wi 

3=1 3=1 


(78) 


(79) 


where iVi is such that = Vje 3 2 . Since eq. (79) holds for alii = 1,..., 2n — 
1, it can be written in compact form as: 

Ww = liw (80) 

hence also w is an eigenvector of W associated with the eigenvalue /x. 
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Now it only remains to demonstrate that v and w are linearly indepen¬ 
dent. One can readily check that, if v t is in the form Vi = r/ 4 e l0, , then 


Vi = Vi 


cos (Qi) 
sin(0i) 


Moreover, observing that Vje 3 2 = rne > ^ i+7r ^ 2 \ then 


Wi = r]i 


cos (9i + 7r/2) 


-sin (Oi) 

sin (Oi + 7t/2) 

— Vi 

cos (Qi) 


(81) 


(82) 


From (81) and (82) is it easy to see that v T w = 0, thus v,w are orthogonal, 
hence independent. To each eigenvalue /r of W there thus correspond an 
identical eigenvalue of W, of geometric multiplicity at least two. Since W 
has 2n — 1 eigenvalues and W has 2(2n — 1) eigenvalues, we conclude that 
to each eigenvalue /r of W there correspond exactly two eigenvalues of W in 
/r. The previous proof also shows how the set of orthogonal eigenvectors of 
W is related to the set of eigenvectors of W. 


8.7 Proof of Theorem [TJ Primal-dual Optimal Pairs 

We prove that, given A G M n , if an x x G T(A) is primal feasible, then x\ is 
primal optimal; moreover, A is dual optimal, and the duality gap is zero. 
By weak duality we know that for any A: 

C(x x ,X )</* (83) 

However, if x\ is primal feasible, by optimality of /*, it must also hold 

/* < fix A ) (84) 


Now we observe that for a feasible x\, the terms in the Lagrangian associated 
to the constraints disappear and C(x\, A) = f(x\). Using the latter equality 
and the inequalities (83) and (84) we get: 

r<f(x x ) = c(x x ,\)<r 


(85) 


which implies f(x x ) = /*, i.e., x x is primal optimal. 
Further, we have that 


d* > min £{x, A) = £{x x , A) = f(x x ) = /*, 

X 

which, combined with weak duality (d* < /*), implies that d* = f* and that 
A attains the dual optimal value. 
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8.8 Numerical Data For the Toy Examples in Section [6] 

Ground truth nodes poses, written as Xi = [pj,0i]: 


Xl = 

[ 0.0000 

-5.0000 

0.2451 ] 

X2 = 

[ 4.7553 

-1.5451 

-0.4496 ] 

X3 = 

2.9389 

4.0451 

0.7361 ] 

X 4 = 

[ -2.9389 

4.0451 

0.3699 ] 

X 5 = 

[ -4.7553 

-1.5451 

-1.7225 ] 

rements 

for each edge 

written as (i,j) 

( 1 , 2 ) 

[ 4.6606 

1.2177 

2.8186 ] 

(2,3) 

[ -4.4199 

4.8043 

0.1519 ; 

(3,4) 

[ -4.1169 

4.9322 

0.5638 ] 

(4,5) 

[ -3.6351 

-5.0908 

-0.5855 ] 

(5,1) 

[ 3.4744 

5.9425 

2.5775 : 


(86) 


•T 


(87) 


References 


[1] R. Aragues, L. Carlone, G. Calafiore, and C. Sagues. Multi-agent lo¬ 
calization from noisy relative pose measurements. In IEEE Inti. Conf. 
on Robotics and Automation (ICRA), pages 364-369, Shanghai, China, 
2011. 

[2] M. Arie-Nachimson, S. Z. Kovalsky, I. Kemelmacher-Shlizerman, 
A. Singer, and R. Basri. Global motion estimation from point matches. 
In 3DIM PVT, 2012. 

[3] A.S. Bandeira, A. Singer, and D.A. Spielman. A Cheeger inequality 
for the graph connection laplacian. SIAM. J. Matrix Anal. & Appl., 
34(4):1611-1630, 2013. 

[4] P. Barooah and J.P. Hespanha. Estimation on graphs from relative 
measurements. Control System Magazine , 27(4) :5T—74, 2007. 

[5] P. Biswas, T. Lian, T. Wang, and Y. Ye. Semidefinite programming 
based algorithms for sensor network localization. ACM Transactions 
on Sensor Networks , 2(2):188—220, 2006. 

[ 6 ] D. Borra, R. Carli, E. Lovisari, F. Fagnani, and S. Zampieri. Au¬ 
tonomous calibration algorithms for planar networks of cameras. Au¬ 
tomatical 2012 . 


46 



[7] S. Boyd and L. Vandenberghe. Convex optimization. Cambridge Uni¬ 
versity Press, 2004. 

[8] G.C. Calafiore, L. Carlone, and M. Wei. Distributed optimization tech¬ 
niques for range localization in networked systems. In IEEE Conf. on 
Decision and Control, pages 2221-2226, 2010. 

[9] Nicholas Carlevaris-Bianco and Ryan M. Eustice. Generic factor-based 
node marginalization and edge sparsification for pose-graph SLAM. In 
IEEE Inti. Conf. on Robotics and Automation (ICRA), pages 5728- 
5735, 2013. 

[10] L. Carlone. Convergence analysis of pose graph optimization via Gauss- 
Newton methods. In IEEE Inti. Conf. on Robotics and Automation 
(ICRA), pages 965-972, 2013. 

[11] L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona. A linear ap¬ 
proximation for graph-based simultaneous localization and mapping. In 
Robotics: Science and Systems (RSS), 2011. 

[12] L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona. A fast and 
accurate approximation for planar pose graph optimization. Inti. J. of 
Robotics Research, 2014. 

[13] L. Carlone and A. Censi. From angular manifolds to the integer lat¬ 
tice: Guaranteed orientation estimation with application to pose graph 
optimization. IEEE Trans. Robotics, 2014. 

[14] L. Carlone and F. Dellaert. Duality-based verification techniques for 2D 
SLAM. In Inti. Conf. on Robotics and Automation (ICRA), accepted, 
2015. 

[15] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert. Initialization tech¬ 
niques for 3D SLAM: a survey on rotation estimation and its use in pose 
graph optimization. In IEEE Inti. Conf. on Robotics and Automation 
(ICRA), 2015. 

[16] G. S. Chirikjian. Stochastic Models, Information Theory, and Lie 
Groups, Volume 2: Analytic Methods and Modern Applications (Ap¬ 
plied and Numerical Harmonic Analysis). Birkhauser, 2012. 

[17] A. Chiuso, G. Picci, and S. Soatto. Wide-sense estimation on the special 
orthogonal group. Commun. Inf. Syst, 8:185-200, 2008. 


47 



[18] F.R.K. Chung. Spectral Graph Theory. American Mathematical Soc., 
CBMS Regional Conference Series in Mathematics, No. 92, 1996. 

[19] J. Costa, N. Patwari, and A. Hero. Distributed weighted¬ 
multidimensional scaling for node localization in sensor networks. ACM 
Transactions on Sensor Networks , 2(l):39-64, 2006. 

[20] M. Cucuringu, Y. Lipman, and A. Singer. Sensor network localization 
by eigenvector synchronization over the euclidean group. ACM Trans¬ 
actions on Sensor Networks, 8(3): 19:1-19:42, 2012. 

[21] M. Cucuringu, A. Singer, and D. Cowburn. Eigenvector synchroniza¬ 
tion, graph rigidity and the molecule problem. Information and Infer¬ 
ence: A Journal of the IMA, l(l):21-67, 2012. 

[22] F. Dellaert, J. Carlson, V. Ila, K. Ni, and C.E. Thorpe. Subgraph- 
preconditioned conjugate gradient for large scale slam. In IEEE/RSJ 
Inti. Conf. on Intelligent Robots and Systems (IROS), 2010. 

[23] L. Doherty, K. Pister, and L. El Ghaoui. Convex position estimation in 
wireless sensornetworks. In IEEE INFOCOM, Volume 3, pages 1655- 
1663, 2001. 

[24] G. Dubbelman and B. Browning. Closed-form online pose-chain slam. 
In IEEE Inti. Conf. on Robotics and Automation (ICRA), 2013. 

[25] G. Dubbelman, I. Esteban, and K. Schutte. Efficient trajectory bend¬ 
ing with applications to loop closure. In IEEE/RSJ Inti. Conf. on 
Intelligent Robots and Systems (IROS), pages 1-7, 2010. 

[26] G. Dubbelman, P. Hansen, B. Browning, and M.B. Dias. Orientation 
only loop-closing with closed-form trajectory bending. In IEEE Inti. 
Conf. on Robotics and Automation (ICRA), 2012. 

[27] T. Duckett, S. Marsland, and J. Shapiro. Fast, on-line learning of 
globally consistent maps. Autonomous Robots, 12(3):287-300, 2002. 

[28] C. Eckart and G. Young. The approximation of one matrix by another 
low rank. Psychometrika, 1:211-218, 1936. 

[29] M. Hazewinkel ed. Complex number, Encyclopedia of Mathematics. 
Springer, 2001. 


48 



[30] T. Eren, O.K. Goldenberg, W. Whiteley, and Y.R. Yang. Rigidity, 
computation, and randomization in network localization. In INFOCOM 
2004■ Twenty-third, AnnualJoint Conference of the IEEE Computer and 
Communications Societies, Volume:4 , pages 2673-2684. IEEE, 2004. 

[31] T. Eren, W. Whiteley, and P.N. Belhumeur. Using angle of arrival 
(bearing) information in network localization. In IEEE Conf. on Deci¬ 
sion and Control , pages 4676-4681, 2006. 

[32] R.M. Eustice, H. Singh, J.J. Leonard, and M.R. Walter. Visually map¬ 
ping the RMS Titanic: Conservative covariance estimates for SLAM 
information filters. Inti. J. of Robotics Research, 25(12): 1223—1242, 
Dec 2006. 

[33] J. Fredriksson and C. Olsson. Simultaneous multiple rotation averaging 
using lagrangian duality. In Asian Conf. on Computer Vision (ACCV), 
2012. 

[34] U. Frese, P. Larsson, and T. Duckett. A multilevel relaxation algorithm 
for simultaneous localisation and mapping. IEEE Trans. Robotics, 
21 (2): 196-207, April 2005. 

[35] V. M. Govindu. Combining two-view constraints for motion estimation. 
In IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), 
pages 218-225, 2001. 

[36] V.M. Govindu. Lie-algebraic averaging for globally consistent motion 
estimation. In IEEE Conf. on Computer Vision and Pattern Recogni¬ 
tion (CVPR), 2004. 

[37] G. Grisetti, C. Stachniss, and W. Burgard. Non-linear constraint net¬ 
work optimization for efficient map learning. Trans, on Intelligent 
Transportation systems, 10(3):428-439, 2009. 

[38] R. Hartley, J. Trumpf, Y. Dai, and H. Li. Rotation averaging. IJCV, 
103(3):267-305, 2013. 

[39] T. Hatanaka, M. Fujita, and F. Bullo. Vision-based cooperative esti¬ 
mation via multi-agent optimization. In IEEE Conf. on Decision and 
Control, 2010. 

[40] S. Huang, Y. Lai, U. Frese, and G. Dissanayake. How far is SLAM from 
a linear least squares problem? In IEEE/RSJ Inti. Conf. on Intelligent 
Robots and Systems (IROS), pages 3011-3016, 2010. 


49 



[41] S. Huang, H. Wang, U. Frese, and G. Dissanayake. On the number of 
local minima to the point feature based SLAM problem. In IEEE Inti. 
Conf. on Robotics and Automation (ICR.A). pages 2074-2079, 2012. 

[42] V. Indelman, E. Nelson, N. Michael, and F. Dellaert. Multi-robot pose 
graph localization and data association from unknown initial relative 
poses via expectation maximization. In IEEE Inti. Conf. on Robotics 
and Automation (ICRA), 2014. 

[43] H. Johannsson, M. Kaess, M. Fallon, and J.J. Leonard. Temporally 
scalable visual SLAM using a reduced pose graph. In IEEE Inti. Conf. 
on Robotics and Automation (ICRA), pages 54-61, 2013. 

[44] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Del¬ 
laert. iSAM2: Incremental smoothing and mapping using the Bayes 
tree. Inti. J. of Robotics Research, 31:217-236, Feb 2012. 

[45] M. Kaess, A. Ranganathan, and F. Dellaert. iSAM: Incremental 
smoothing and mapping. IEEE Trans. Robotics, 24(6): 1365-1378, Dec 
2008. 

[46] B. Kim, M. Kaess, L. Fletcher, J. Leonard, A. Bachrach, N. Roy, and 
S. Teller. Multiple relative pose graphs for robust cooperative mapping. 
In IEEE Inti. Conf. on Robotics and Automation (ICRA), pages 3185- 
3192, Anchorage, Alaska, May 2010. 

[47] J. Knuth and P. Barooah. Collaborative 3D localization of robots from 
relative pose measurements using gradient descent on manifolds. In 
IEEE Inti. Conf. on Robotics and Automation (ICRA), pages 1101 
1106, 2012. 

[48] J. Knuth and P. Barooah. Collaborative localization with heterogeneous 
inter-robot measurements by Riemannian optimization. In IEEE Inti. 
Conf. on Robotics and Automation (ICRA), 2013. 

[49] J. Knuth and P. Barooah. Error growth in position estimation from 
noisy relative pose measurements. Robotics and Autonomous Systems , 
61(3):229-224, 2013. 

[50] K. Konolige. Large-scale map-making. In Proc. 21 th AAAI National 
Conference on AI, San Jose, CA, 2004. 


50 



[51] R. Kiimmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard. 
g2o: A general framework for graph optimization. In Proc. of the IEEE 
Int. Conf. on Robotics and Automation (ICRA), Shanghai, China, May 
2011. 

[52] K. Levenberg. A method for the solution of certain nonlinear problems 
in least squares. Quart. Appl. Math , 2(2):164—168, 1944. 

[53] F. Lu and E. Milios. Globally consistent range scan alignment for 
environment mapping. Autonomous Robots , pages 333-349, Apr 1997. 

[54] G. Mao, B. Fidan, and B. Anderson. Wireless sensor network localiza¬ 
tion techniques. Computer Networks, 51(10) :2529-2553, 2007. 

[55] D. Martinec and T. Pajdla. Robust rotation and translation estimation 
in multiview reconstruction. In IEEE Conf. on Computer Vision and 
Pattern Recognition (CVPR), pages 1-8, 2007. 

[56] R. Olfati-Saber. Swarms on sphere: A programmable swarm with syn¬ 
chronous behaviors like oscillator networks. In IEEE Conf. on Decision 
and Control, pages 5060-5066, 2006. 

[57] E. Olson, J. Leonard, and S. Teller. Fast iterative alignment of pose 
graphs with poor initial estimates. In IEEE Inti. Conf. on Robotics and 
Automation (ICRA), pages 2262-2269, May 2006. 

[58] J. R. Peters, D. Borra, B. Paden, and F. Bullo. Sensor network local¬ 
ization on the group of 3D displacements. SIAM Journal on Control 
and Optimization, submitted, 2014. 

[59] G. Piovan, I. Shames, B. Fidan, F. Bullo, and B. Anderson. On frame 
and orientation localization for relative sensing networks. Automatica, 
49(1) :206—213, 2013. 

[60] N. Reff. Spectral properties of complex unit graphs. arXiv, 2011. 

[61] D.M. Rosen, M. Kaess, and J.J. Leonard. An incremental trust-region 
method for robust online sparse least-squares estimation. In IEEE Inti. 
Conf. on Robotics and Automation (ICRA), pages 1262-1269, St. Paul, 
MN, May 2012. 

[62] D.M. Rosen, M. Kaess, and J.J. Leonard. RISE: An incremental trust- 
region method for robust online sparse least-squares estimation. IEEE 
Trans. Robotics, 2014. 


51 



[63] W.J. Russell, D.J. Klein, and J.P. Hespanha. Optimal estimation on 
the graph cycle space. 59(6):2834-2846, 2011. 

[64] A. Sarlette and R. Sepulchre. Consensus optimization on manifolds. 
SIAM J. Control and Optimization, 48(l):56-76, 2009. 

[65] J. Saunderson, P.A. Parrilo, and A. Willsky. Semidefinite descrip¬ 
tions of the convex hull of rotation matrices. ArXiv preprint: 

http://arxiv.org/abs/i 403 . 49 i 4 , 2014. 

[66] J. Saunderson, P.A. Parrilo, and A. Willsky. Semidefinite relaxations 
for optimization problems over rotation matrices. In IEEE Conf. on 
Decision and Control, May 2014. 

[67] Al. Schrijver. Theory of Linear and Integer Programming. Wiley, 1998. 

[68] G.C. Sharp, S.W. Lee, and D.K. Wehe. Multiview registration of 3D 
scenes by minimizing error between coordinate frames. IEEE Trans. 
Pattern Anal. Machine Intell., 26(8): 1037-1050, 2004. 

[69] A. Singer. A remark on global positioning from local distances. Pro¬ 
ceedings of the National Academy of Sciences, 105(28):9507-9511, 2008. 

[70] A. Singer. Angular synchronization by eigenvectors and semidefinite 
programming. Appl. Comput. Harmon. Anal., 30:20-36, 2010. 

[71] A. Singer and Y. Shkolnisky. Three-dimensional structure determina¬ 
tion from common lines in Cryo-EM by eigenvectors and semidefinite 
programming. SIAM J. Imaging Sciences, pages 543-572, 2011. 

[72] R. Stanfield. Statistical theory of DF finding. Journal of IEE, 
94(5):762-770, 1947. 

[73] J. Thunberg, E. Montijano, and X. Hu. Distributed attitude synchro¬ 
nization control. In IEEE Conf. on Decision and Control, 2011. 

[74] R. Tron, B. Afsari, and R. Vidal. Intrinsic consensus on SO(3) with 
almost global convergence. In IEEE Conf. on Decision and Control, 
2012. 

[75] R. Tron, B. Afsari, and R. Vidal. Riemannian consensus for manifolds 
with bounded curvature. IEEE Trans, on Automatic Control, 2012. 


52 



[76] R. Tron, L. Carlone, F. Dellaert, and K. Daniilidis. Rigid components 
identification and rigidity enforcement in bearing-only localization us¬ 
ing the graph cycle basis. Rr American Control Conference, 2015. 

[77] R. Tron and R. Vidal. Distributed image-based 3-D localization in 
camera networks. In IEEE Conf. on Decision and Control, 2009. 

[78] H. Wang, G. Hu, S. Huang, and G. Dissanayake. On the structure of 
nonlinearities in pose graph SLAM. In Robotics: Science and Systems 
(RSS), 2012. 

[79] L. Wang and A. Singer. Exact and stable recovery of rotations for 
robust synchronization. Information and Inference: A Journal of the 
IMA, 30, 2013. 

[80] S. Zhang. Quadratic maximization and semidefinite relaxation. Math. 
Programming, Ser. A, 87:453-465, 2000. 


53 



